summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--drivers/usb/gadget/f_mass_storage.c58
-rw-r--r--drivers/usb/gadget/file_storage.c62
2 files changed, 76 insertions, 44 deletions
diff --git a/drivers/usb/gadget/f_mass_storage.c b/drivers/usb/gadget/f_mass_storage.c
index c39d58860fa..860e15ac8f2 100644
--- a/drivers/usb/gadget/f_mass_storage.c
+++ b/drivers/usb/gadget/f_mass_storage.c
@@ -1873,17 +1873,14 @@ static int check_command(struct fsg_common *common, int cmnd_size,
common->lun, lun);
/* Check the LUN */
- if (common->lun < common->nluns) {
- curlun = &common->luns[common->lun];
- common->curlun = curlun;
+ curlun = common->curlun;
+ if (curlun) {
if (common->cmnd[0] != REQUEST_SENSE) {
curlun->sense_data = SS_NO_SENSE;
curlun->sense_data_info = 0;
curlun->info_valid = 0;
}
} else {
- common->curlun = NULL;
- curlun = NULL;
common->bad_lun_okay = 0;
/*
@@ -1929,6 +1926,17 @@ static int check_command(struct fsg_common *common, int cmnd_size,
return 0;
}
+/* wrapper of check_command for data size in blocks handling */
+static int check_command_size_in_blocks(struct fsg_common *common,
+ int cmnd_size, enum data_direction data_dir,
+ unsigned int mask, int needs_medium, const char *name)
+{
+ if (common->curlun)
+ common->data_size_from_cmnd <<= common->curlun->blkbits;
+ return check_command(common, cmnd_size, data_dir,
+ mask, needs_medium, name);
+}
+
static int do_scsi_command(struct fsg_common *common)
{
struct fsg_buffhd *bh;
@@ -2011,9 +2019,9 @@ static int do_scsi_command(struct fsg_common *common)
case READ_6:
i = common->cmnd[4];
- common->data_size_from_cmnd = (i == 0 ? 256 : i) <<
- common->curlun->blkbits;
- reply = check_command(common, 6, DATA_DIR_TO_HOST,
+ common->data_size_from_cmnd = (i == 0) ? 256 : i;
+ reply = check_command_size_in_blocks(common, 6,
+ DATA_DIR_TO_HOST,
(7<<1) | (1<<4), 1,
"READ(6)");
if (reply == 0)
@@ -2022,9 +2030,9 @@ static int do_scsi_command(struct fsg_common *common)
case READ_10:
common->data_size_from_cmnd =
- get_unaligned_be16(&common->cmnd[7]) <<
- common->curlun->blkbits;
- reply = check_command(common, 10, DATA_DIR_TO_HOST,
+ get_unaligned_be16(&common->cmnd[7]);
+ reply = check_command_size_in_blocks(common, 10,
+ DATA_DIR_TO_HOST,
(1<<1) | (0xf<<2) | (3<<7), 1,
"READ(10)");
if (reply == 0)
@@ -2033,9 +2041,9 @@ static int do_scsi_command(struct fsg_common *common)
case READ_12:
common->data_size_from_cmnd =
- get_unaligned_be32(&common->cmnd[6]) <<
- common->curlun->blkbits;
- reply = check_command(common, 12, DATA_DIR_TO_HOST,
+ get_unaligned_be32(&common->cmnd[6]);
+ reply = check_command_size_in_blocks(common, 12,
+ DATA_DIR_TO_HOST,
(1<<1) | (0xf<<2) | (0xf<<6), 1,
"READ(12)");
if (reply == 0)
@@ -2134,9 +2142,9 @@ static int do_scsi_command(struct fsg_common *common)
case WRITE_6:
i = common->cmnd[4];
- common->data_size_from_cmnd = (i == 0 ? 256 : i) <<
- common->curlun->blkbits;
- reply = check_command(common, 6, DATA_DIR_FROM_HOST,
+ common->data_size_from_cmnd = (i == 0) ? 256 : i;
+ reply = check_command_size_in_blocks(common, 6,
+ DATA_DIR_FROM_HOST,
(7<<1) | (1<<4), 1,
"WRITE(6)");
if (reply == 0)
@@ -2145,9 +2153,9 @@ static int do_scsi_command(struct fsg_common *common)
case WRITE_10:
common->data_size_from_cmnd =
- get_unaligned_be16(&common->cmnd[7]) <<
- common->curlun->blkbits;
- reply = check_command(common, 10, DATA_DIR_FROM_HOST,
+ get_unaligned_be16(&common->cmnd[7]);
+ reply = check_command_size_in_blocks(common, 10,
+ DATA_DIR_FROM_HOST,
(1<<1) | (0xf<<2) | (3<<7), 1,
"WRITE(10)");
if (reply == 0)
@@ -2156,9 +2164,9 @@ static int do_scsi_command(struct fsg_common *common)
case WRITE_12:
common->data_size_from_cmnd =
- get_unaligned_be32(&common->cmnd[6]) <<
- common->curlun->blkbits;
- reply = check_command(common, 12, DATA_DIR_FROM_HOST,
+ get_unaligned_be32(&common->cmnd[6]);
+ reply = check_command_size_in_blocks(common, 12,
+ DATA_DIR_FROM_HOST,
(1<<1) | (0xf<<2) | (0xf<<6), 1,
"WRITE(12)");
if (reply == 0)
@@ -2273,6 +2281,10 @@ static int received_cbw(struct fsg_dev *fsg, struct fsg_buffhd *bh)
if (common->data_size == 0)
common->data_dir = DATA_DIR_NONE;
common->lun = cbw->Lun;
+ if (common->lun >= 0 && common->lun < common->nluns)
+ common->curlun = &common->luns[common->lun];
+ else
+ common->curlun = NULL;
common->tag = cbw->Tag;
return 0;
}
diff --git a/drivers/usb/gadget/file_storage.c b/drivers/usb/gadget/file_storage.c
index 11b5196284a..e8ff2f1c06c 100644
--- a/drivers/usb/gadget/file_storage.c
+++ b/drivers/usb/gadget/file_storage.c
@@ -2297,19 +2297,17 @@ static int check_command(struct fsg_dev *fsg, int cmnd_size,
DBG(fsg, "using LUN %d from CBW, "
"not LUN %d from CDB\n",
fsg->lun, lun);
- } else
- fsg->lun = lun; // Use LUN from the command
+ }
/* Check the LUN */
- if (fsg->lun < fsg->nluns) {
- fsg->curlun = curlun = &fsg->luns[fsg->lun];
+ curlun = fsg->curlun;
+ if (curlun) {
if (fsg->cmnd[0] != REQUEST_SENSE) {
curlun->sense_data = SS_NO_SENSE;
curlun->sense_data_info = 0;
curlun->info_valid = 0;
}
} else {
- fsg->curlun = curlun = NULL;
fsg->bad_lun_okay = 0;
/* INQUIRY and REQUEST SENSE commands are explicitly allowed
@@ -2351,6 +2349,16 @@ static int check_command(struct fsg_dev *fsg, int cmnd_size,
return 0;
}
+/* wrapper of check_command for data size in blocks handling */
+static int check_command_size_in_blocks(struct fsg_dev *fsg, int cmnd_size,
+ enum data_direction data_dir, unsigned int mask,
+ int needs_medium, const char *name)
+{
+ if (fsg->curlun)
+ fsg->data_size_from_cmnd <<= fsg->curlun->blkbits;
+ return check_command(fsg, cmnd_size, data_dir,
+ mask, needs_medium, name);
+}
static int do_scsi_command(struct fsg_dev *fsg)
{
@@ -2425,26 +2433,27 @@ static int do_scsi_command(struct fsg_dev *fsg)
case READ_6:
i = fsg->cmnd[4];
- fsg->data_size_from_cmnd = (i == 0 ? 256 : i) << fsg->curlun->blkbits;
- if ((reply = check_command(fsg, 6, DATA_DIR_TO_HOST,
+ fsg->data_size_from_cmnd = (i == 0) ? 256 : i;
+ if ((reply = check_command_size_in_blocks(fsg, 6,
+ DATA_DIR_TO_HOST,
(7<<1) | (1<<4), 1,
"READ(6)")) == 0)
reply = do_read(fsg);
break;
case READ_10:
- fsg->data_size_from_cmnd =
- get_unaligned_be16(&fsg->cmnd[7]) << fsg->curlun->blkbits;
- if ((reply = check_command(fsg, 10, DATA_DIR_TO_HOST,
+ fsg->data_size_from_cmnd = get_unaligned_be16(&fsg->cmnd[7]);
+ if ((reply = check_command_size_in_blocks(fsg, 10,
+ DATA_DIR_TO_HOST,
(1<<1) | (0xf<<2) | (3<<7), 1,
"READ(10)")) == 0)
reply = do_read(fsg);
break;
case READ_12:
- fsg->data_size_from_cmnd =
- get_unaligned_be32(&fsg->cmnd[6]) << fsg->curlun->blkbits;
- if ((reply = check_command(fsg, 12, DATA_DIR_TO_HOST,
+ fsg->data_size_from_cmnd = get_unaligned_be32(&fsg->cmnd[6]);
+ if ((reply = check_command_size_in_blocks(fsg, 12,
+ DATA_DIR_TO_HOST,
(1<<1) | (0xf<<2) | (0xf<<6), 1,
"READ(12)")) == 0)
reply = do_read(fsg);
@@ -2529,26 +2538,27 @@ static int do_scsi_command(struct fsg_dev *fsg)
case WRITE_6:
i = fsg->cmnd[4];
- fsg->data_size_from_cmnd = (i == 0 ? 256 : i) << fsg->curlun->blkbits;
- if ((reply = check_command(fsg, 6, DATA_DIR_FROM_HOST,
+ fsg->data_size_from_cmnd = (i == 0) ? 256 : i;
+ if ((reply = check_command_size_in_blocks(fsg, 6,
+ DATA_DIR_FROM_HOST,
(7<<1) | (1<<4), 1,
"WRITE(6)")) == 0)
reply = do_write(fsg);
break;
case WRITE_10:
- fsg->data_size_from_cmnd =
- get_unaligned_be16(&fsg->cmnd[7]) << fsg->curlun->blkbits;
- if ((reply = check_command(fsg, 10, DATA_DIR_FROM_HOST,
+ fsg->data_size_from_cmnd = get_unaligned_be16(&fsg->cmnd[7]);
+ if ((reply = check_command_size_in_blocks(fsg, 10,
+ DATA_DIR_FROM_HOST,
(1<<1) | (0xf<<2) | (3<<7), 1,
"WRITE(10)")) == 0)
reply = do_write(fsg);
break;
case WRITE_12:
- fsg->data_size_from_cmnd =
- get_unaligned_be32(&fsg->cmnd[6]) << fsg->curlun->blkbits;
- if ((reply = check_command(fsg, 12, DATA_DIR_FROM_HOST,
+ fsg->data_size_from_cmnd = get_unaligned_be32(&fsg->cmnd[6]);
+ if ((reply = check_command_size_in_blocks(fsg, 12,
+ DATA_DIR_FROM_HOST,
(1<<1) | (0xf<<2) | (0xf<<6), 1,
"WRITE(12)")) == 0)
reply = do_write(fsg);
@@ -2715,7 +2725,17 @@ static int get_next_command(struct fsg_dev *fsg)
memcpy(fsg->cmnd, fsg->cbbuf_cmnd, fsg->cmnd_size);
fsg->cbbuf_cmnd_size = 0;
spin_unlock_irq(&fsg->lock);
+
+ /* Use LUN from the command */
+ fsg->lun = fsg->cmnd[1] >> 5;
}
+
+ /* Update current lun */
+ if (fsg->lun >= 0 && fsg->lun < fsg->nluns)
+ fsg->curlun = &fsg->luns[fsg->lun];
+ else
+ fsg->curlun = NULL;
+
return rc;
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-26 16:25:24 +0000