1 files changed, 110 insertions, 4 deletions

diff --git a/drivers/scsi/ufs/ufs_bsg.c b/drivers/scsi/ufs/ufs_bsg.c
index 1036c520d349..306e5f11a818 100644
--- a/drivers/scsi/ufs/ufs_bsg.c
+++ b/drivers/scsi/ufs/ufs_bsg.c
@@ -6,19 +6,125 @@
  */
 #include "ufs_bsg.h"
 
+static int ufs_bsg_get_query_desc_size(struct ufs_hba *hba, int *desc_len,
+				       struct utp_upiu_query *qr)
+{
+	int desc_size = be16_to_cpu(qr->length);
+	int desc_id = qr->idn;
+	int ret;
+
+	if (desc_size <= 0)
+		return -EINVAL;
+
+	ret = ufshcd_map_desc_id_to_length(hba, desc_id, desc_len);
+	if (ret || !*desc_len)
+		return -EINVAL;
+
+	*desc_len = min_t(int, *desc_len, desc_size);
+
+	return 0;
+}
+
+static int ufs_bsg_verify_query_size(struct ufs_hba *hba,
+				     unsigned int request_len,
+				     unsigned int reply_len,
+				     int desc_len, enum query_opcode desc_op)
+{
+	int min_req_len = sizeof(struct ufs_bsg_request);
+	int min_rsp_len = sizeof(struct ufs_bsg_reply);
+
+	if (desc_op == UPIU_QUERY_OPCODE_WRITE_DESC)
+		min_req_len += desc_len;
+
+	if (min_req_len > request_len || min_rsp_len > reply_len) {
+		dev_err(hba->dev, "not enough space assigned\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int ufs_bsg_verify_query_params(struct ufs_hba *hba,
+				       struct ufs_bsg_request *bsg_request,
+				       unsigned int request_len,
+				       unsigned int reply_len,
+				       uint8_t *desc_buff, int *desc_len,
+				       enum query_opcode desc_op)
+{
+	struct utp_upiu_query *qr;
+
+	if (desc_op == UPIU_QUERY_OPCODE_READ_DESC) {
+		dev_err(hba->dev, "unsupported opcode %d\n", desc_op);
+		return -ENOTSUPP;
+	}
+
+	if (desc_op != UPIU_QUERY_OPCODE_WRITE_DESC)
+		goto out;
+
+	qr = &bsg_request->upiu_req.qr;
+	if (ufs_bsg_get_query_desc_size(hba, desc_len, qr)) {
+		dev_err(hba->dev, "Illegal desc size\n");
+		return -EINVAL;
+	}
+
+	if (ufs_bsg_verify_query_size(hba, request_len, reply_len, *desc_len,
+				      desc_op))
+		return -EINVAL;
+
+	desc_buff = (uint8_t *)(bsg_request + 1);
+
+out:
+	return 0;
+}
 
 static int ufs_bsg_request(struct bsg_job *job)
 {
 	struct ufs_bsg_request *bsg_request = job->request;
 	struct ufs_bsg_reply *bsg_reply = job->reply;
-	int ret = -ENOTSUPP;
+	struct ufs_hba *hba = shost_priv(dev_to_shost(job->dev->parent));
+	unsigned int req_len = job->request_len;
+	unsigned int reply_len = job->reply_len;
+	int msgcode;
+	uint8_t *desc_buff = NULL;
+	int desc_len = 0;
+	enum query_opcode desc_op = UPIU_QUERY_OPCODE_NOP;
+	int ret;
+
+	ret = ufs_bsg_verify_query_size(hba, req_len, reply_len, 0, desc_op);
+	if (ret)
+		goto out;
 
 	bsg_reply->reply_payload_rcv_len = 0;
 
-	/* Do Nothing for now */
-	dev_err(job->dev, "unsupported message_code 0x%x\n",
-		bsg_request->msgcode);
+	msgcode = bsg_request->msgcode;
+	switch (msgcode) {
+	case UPIU_TRANSACTION_QUERY_REQ:
+		desc_op = bsg_request->upiu_req.qr.opcode;
+		ret = ufs_bsg_verify_query_params(hba, bsg_request, req_len,
+						  reply_len, desc_buff,
+						  &desc_len, desc_op);
+		if (ret)
+			goto out;
+
+		/* fall through */
+	case UPIU_TRANSACTION_NOP_OUT:
+	case UPIU_TRANSACTION_TASK_REQ:
+		ret = ufshcd_exec_raw_upiu_cmd(hba, &bsg_request->upiu_req,
+					       &bsg_reply->upiu_rsp, msgcode,
+					       desc_buff, &desc_len, desc_op);
+		if (ret)
+			dev_err(hba->dev,
+				"exe raw upiu: error code %d\n", ret);
+
+		break;
+	default:
+		ret = -ENOTSUPP;
+		dev_err(hba->dev, "unsupported msgcode 0x%x\n", msgcode);
+
+		break;
+	}
 
+out:
 	bsg_reply->result = ret;
 	job->reply_len = sizeof(struct ufs_bsg_reply) +
 			 bsg_reply->reply_payload_rcv_len;