@@ -19,18 +19,129 @@ struct ufs_bsg {
.bsg_request = ufs_bsg_request,
};
+static int ufs_bsg_get_query_desc_size(struct utp_upiu_query *qr,
+ int *buff_len)
+{
+ int desc_size = be16_to_cpu(qr->length);
+ int desc_id = qr->idn;
+ int ret = 0;
+
+ if (qr->opcode != UPIU_QUERY_OPCODE_WRITE_DESC ||
+ desc_size <= 0)
+ return -EINVAL;
+
+ ret = ufshcd_map_desc_id_to_length(bsg_host->hba, desc_id, buff_len);
+
+ if (ret || !buff_len)
+ return -EINVAL;
+
+ *buff_len = min_t(int, *buff_len, desc_size);
+
+ return ret;
+}
+
+static int ufs_bsg_verify_query_size(unsigned int request_len,
+ unsigned int reply_len,
+ int rw, int buff_len)
+{
+ int min_req_len = sizeof(struct ufs_bsg_request);
+ int min_rsp_len = sizeof(struct ufs_bsg_reply);
+
+ if (rw == UFS_BSG_NOP)
+ goto null_buff;
+
+ if (rw == WRITE)
+ min_req_len += buff_len;
+
+null_buff:
+ if (min_req_len > request_len)
+ return -EINVAL;
+
+ if (min_rsp_len > reply_len)
+ return -EINVAL;
+
+ 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;
+ unsigned int request_len = job->request_len;
+ unsigned int reply_len = job->reply_len;
+ struct utp_upiu_query *qr;
+ __be32 *req_upiu = NULL;
+ __be32 *rsp_upiu = NULL;
+ int msgcode;
+ uint8_t *desc_buff = NULL;
+ int buff_len = 0;
+ int rw = UFS_BSG_NOP;
+ int ret;
+ ret = ufs_bsg_verify_query_size(request_len, reply_len, rw, buff_len);
+ if (ret) {
+ dev_err(job->dev, "not enough space assigned\n");
+ 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:
+ qr = &bsg_request->tsf.qr;
+ if (qr->opcode == UPIU_QUERY_OPCODE_READ_DESC)
+ goto not_supported;
+
+ if (ufs_bsg_get_query_desc_size(qr, &buff_len))
+ goto null_desc_buff;
+
+ if (qr->opcode == UPIU_QUERY_OPCODE_WRITE_DESC) {
+ rw = WRITE;
+ desc_buff = ((uint8_t *)bsg_request) +
+ sizeof(struct ufs_bsg_request);
+ }
+
+null_desc_buff:
+ /* fall through */
+ case UPIU_TRANSACTION_NOP_OUT:
+ case UPIU_TRANSACTION_TASK_REQ:
+ /* Now that we know if its a read or write, verify again */
+ if (rw != UFS_BSG_NOP || buff_len) {
+ ret = ufs_bsg_verify_query_size(request_len, reply_len,
+ rw, buff_len);
+ if (ret) {
+ dev_err(job->dev,
+ "not enough space assigned\n");
+ goto out;
+ }
+ }
+
+ req_upiu = (__be32 *)&bsg_request->header;
+ rsp_upiu = (__be32 *)&bsg_reply->header;
+ ret = ufshcd_exec_raw_upiu_cmd(bsg_host->hba,
+ req_upiu, rsp_upiu, msgcode,
+ desc_buff, &buff_len, rw);
+
+ break;
+ case UPIU_TRANSACTION_UIC_CMD:
+ /* later */
+ case UPIU_TRANSACTION_COMMAND:
+ case UPIU_TRANSACTION_DATA_OUT:
+not_supported:
+ /* for the time being, we do not support data transfer upiu */
+ ret = -ENOTSUPP;
+ dev_err(job->dev, "unsupported msgcode 0x%x\n", msgcode);
+
+ break;
+ default:
+ ret = -EINVAL;
+
+ break;
+ }
+
+ bsg_reply->reply_payload_rcv_len = buff_len;
+out:
bsg_reply->result = ret;
if (ret)
job->reply_len = sizeof(uint32_t);
@@ -13,6 +13,7 @@
#include "ufshcd.h"
#include "ufs.h"
+#define UFS_BSG_NOP (-1)
#define UPIU_TRANSACTION_UIC_CMD 0x1F
enum {
Do that for the currently supported UPIUs: query, nop out, and task management. We do not support UPIU of type scsi command yet, while we are using the job's request and reply pointers to hold the payload. We will look into it in later patches. We might need to elaborate the raw upiu api for that. We also still not supporting uic commands: For first phase, we plan to use the existing api, and send only uic commands that are already supported. Anyway, all that will come in the next patch. Signed-off-by: Avri Altman <avri.altman@wdc.com> --- drivers/scsi/ufs/ufs_bsg.c | 119 +++++++++++++++++++++++++++++++++++++++++++-- drivers/scsi/ufs/ufs_bsg.h | 1 + 2 files changed, 116 insertions(+), 4 deletions(-)