diff mbox series

[4/5] usb: gadget: UMS: support multiple sector sizes

Message ID 20240131-b4-qcom-usb-v1-4-6438b2a2285e@linaro.org
State Superseded
Headers show
Series Qualcomm DWC3 USB support | expand

Commit Message

Caleb Connolly Jan. 31, 2024, 2:57 p.m. UTC
UFS storage often uses a 4096-byte sector size, add support for dynamic
sector sizes based loosely on the Linux implementation.

Signed-off-by: Caleb Connolly <caleb.connolly@linaro.org>
---
 cmd/usb_mass_storage.c              |   4 --
 drivers/usb/gadget/f_mass_storage.c | 101 ++++++++++++++++++++----------------
 drivers/usb/gadget/storage_common.c |  12 +++--
 include/usb_mass_storage.h          |   1 -
 4 files changed, 65 insertions(+), 53 deletions(-)

Comments

Mattijs Korpershoek Feb. 1, 2024, 1:49 p.m. UTC | #1
Hi Caleb,

Thank you for the patch.

On mer., janv. 31, 2024 at 14:57, Caleb Connolly <caleb.connolly@linaro.org> wrote:

> UFS storage often uses a 4096-byte sector size, add support for dynamic
> sector sizes based loosely on the Linux implementation.
>
> Signed-off-by: Caleb Connolly <caleb.connolly@linaro.org>

Reviewed-by: Mattijs Korpershoek <mkorpershoek@baylibre.com>

> ---
>  cmd/usb_mass_storage.c              |   4 --
>  drivers/usb/gadget/f_mass_storage.c | 101 ++++++++++++++++++++----------------
>  drivers/usb/gadget/storage_common.c |  12 +++--
>  include/usb_mass_storage.h          |   1 -
>  4 files changed, 65 insertions(+), 53 deletions(-)
>
> diff --git a/cmd/usb_mass_storage.c b/cmd/usb_mass_storage.c
> index a8ddeb494628..751701fe73af 100644
> --- a/cmd/usb_mass_storage.c
> +++ b/cmd/usb_mass_storage.c
> @@ -88,10 +88,6 @@ static int ums_init(const char *devtype, const char *devnums_part_str)
>  		if (!strchr(devnum_part_str, ':'))
>  			partnum = 0;
>  
> -		/* f_mass_storage.c assumes SECTOR_SIZE sectors */
> -		if (block_dev->blksz != SECTOR_SIZE)
> -			goto cleanup;
> -
>  		ums_new = realloc(ums, (ums_count + 1) * sizeof(*ums));
>  		if (!ums_new)
>  			goto cleanup;
> diff --git a/drivers/usb/gadget/f_mass_storage.c b/drivers/usb/gadget/f_mass_storage.c
> index c725aed3f626..d880928044f4 100644
> --- a/drivers/usb/gadget/f_mass_storage.c
> +++ b/drivers/usb/gadget/f_mass_storage.c
> @@ -724,12 +724,13 @@ static int do_read(struct fsg_common *common)
>  		curlun->sense_data = SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
>  		return -EINVAL;
>  	}
> -	file_offset = ((loff_t) lba) << 9;
> +	file_offset = ((loff_t)lba) << curlun->blkbits;
>  
>  	/* Carry out the file reads */
>  	amount_left = common->data_size_from_cmnd;
> -	if (unlikely(amount_left == 0))
> +	if (unlikely(amount_left == 0)) {
>  		return -EIO;		/* No default reply */
> +	}
>  
>  	for (;;) {
>  
> @@ -768,13 +769,13 @@ static int do_read(struct fsg_common *common)
>  
>  		/* Perform the read */
>  		rc = ums[common->lun].read_sector(&ums[common->lun],
> -				      file_offset / SECTOR_SIZE,
> -				      amount / SECTOR_SIZE,
> +				      file_offset / curlun->blksize,
> +				      amount / curlun->blksize,
>  				      (char __user *)bh->buf);
>  		if (!rc)
>  			return -EIO;
>  
> -		nread = rc * SECTOR_SIZE;
> +		nread = rc * curlun->blksize;
>  
>  		VLDBG(curlun, "file read %u @ %llu -> %d\n", amount,
>  				(unsigned long long) file_offset,
> @@ -787,7 +788,7 @@ static int do_read(struct fsg_common *common)
>  		} else if (nread < amount) {
>  			LDBG(curlun, "partial file read: %d/%u\n",
>  					(int) nread, amount);
> -			nread -= (nread & 511);	/* Round down to a block */
> +			nread -= (nread & (curlun->blksize - 1));	/* Round down to a block */
>  		}
>  		file_offset  += nread;
>  		amount_left  -= nread;
> @@ -861,7 +862,7 @@ static int do_write(struct fsg_common *common)
>  
>  	/* Carry out the file writes */
>  	get_some_more = 1;
> -	file_offset = usb_offset = ((loff_t) lba) << 9;
> +	file_offset = usb_offset = ((loff_t)lba) << curlun->blkbits;
>  	amount_left_to_req = common->data_size_from_cmnd;
>  	amount_left_to_write = common->data_size_from_cmnd;
>  
> @@ -893,7 +894,7 @@ static int do_write(struct fsg_common *common)
>  				curlun->info_valid = 1;
>  				continue;
>  			}
> -			amount -= (amount & 511);
> +			amount -= (amount & (curlun->blksize - 1));
>  			if (amount == 0) {
>  
>  				/* Why were we were asked to transfer a
> @@ -942,12 +943,12 @@ static int do_write(struct fsg_common *common)
>  
>  			/* Perform the write */
>  			rc = ums[common->lun].write_sector(&ums[common->lun],
> -					       file_offset / SECTOR_SIZE,
> -					       amount / SECTOR_SIZE,
> +					       file_offset / curlun->blksize,
> +					       amount / curlun->blksize,
>  					       (char __user *)bh->buf);
>  			if (!rc)
>  				return -EIO;
> -			nwritten = rc * SECTOR_SIZE;
> +			nwritten = rc * curlun->blksize;
>  
>  			VLDBG(curlun, "file write %u @ %llu -> %d\n", amount,
>  					(unsigned long long) file_offset,
> @@ -960,7 +961,7 @@ static int do_write(struct fsg_common *common)
>  			} else if (nwritten < amount) {
>  				LDBG(curlun, "partial file write: %d/%u\n",
>  						(int) nwritten, amount);
> -				nwritten -= (nwritten & 511);
> +				nwritten -= (nwritten & (curlun->blksize - 1));
>  				/* Round down to a block */
>  			}
>  			file_offset += nwritten;
> @@ -1034,8 +1035,8 @@ static int do_verify(struct fsg_common *common)
>  		return -EIO;		/* No default reply */
>  
>  	/* Prepare to carry out the file verify */
> -	amount_left = verification_length << 9;
> -	file_offset = ((loff_t) lba) << 9;
> +	amount_left = verification_length << curlun->blkbits;
> +	file_offset = ((loff_t) lba) << curlun->blkbits;
>  
>  	/* Write out all the dirty buffers before invalidating them */
>  
> @@ -1058,12 +1059,12 @@ static int do_verify(struct fsg_common *common)
>  
>  		/* Perform the read */
>  		rc = ums[common->lun].read_sector(&ums[common->lun],
> -				      file_offset / SECTOR_SIZE,
> -				      amount / SECTOR_SIZE,
> +				      file_offset / curlun->blksize,
> +				      amount / curlun->blksize,
>  				      (char __user *)bh->buf);
>  		if (!rc)
>  			return -EIO;
> -		nread = rc * SECTOR_SIZE;
> +		nread = rc * curlun->blksize;
>  
>  		VLDBG(curlun, "file read %u @ %llu -> %d\n", amount,
>  				(unsigned long long) file_offset,
> @@ -1075,7 +1076,7 @@ static int do_verify(struct fsg_common *common)
>  		} else if (nread < amount) {
>  			LDBG(curlun, "partial file verify: %d/%u\n",
>  					(int) nread, amount);
> -			nread -= (nread & 511);	/* Round down to a sector */
> +			nread -= (nread & (curlun->blksize - 1));	/* Round down to a sector */
>  		}
>  		if (nread == 0) {
>  			curlun->sense_data = SS_UNRECOVERED_READ_ERROR;
> @@ -1183,7 +1184,7 @@ static int do_read_capacity(struct fsg_common *common, struct fsg_buffhd *bh)
>  
>  	put_unaligned_be32(curlun->num_sectors - 1, &buf[0]);
>  						/* Max logical block */
> -	put_unaligned_be32(512, &buf[4]);	/* Block length */
> +	put_unaligned_be32(curlun->blksize, &buf[4]);	/* Block length */
>  	return 8;
>  }
>  
> @@ -1370,7 +1371,7 @@ static int do_read_format_capacities(struct fsg_common *common,
>  
>  	put_unaligned_be32(curlun->num_sectors, &buf[0]);
>  						/* Number of blocks */
> -	put_unaligned_be32(512, &buf[4]);	/* Block length */
> +	put_unaligned_be32(curlun->blksize, &buf[4]);	/* Block length */
>  	buf[4] = 0x02;				/* Current capacity */
>  	return 12;
>  }
> @@ -1781,6 +1782,16 @@ 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)
> +{
> +	common->data_size_from_cmnd <<= common->luns[common->lun].blkbits;
> +	return check_command(common, cmnd_size, data_dir,
> +			mask, needs_medium, name);
> +}
> +
>  
>  static int do_scsi_command(struct fsg_common *common)
>  {
> @@ -1865,30 +1876,30 @@ static int do_scsi_command(struct fsg_common *common)
>  
>  	case SC_READ_6:
>  		i = common->cmnd[4];
> -		common->data_size_from_cmnd = (i == 0 ? 256 : i) << 9;
> -		reply = check_command(common, 6, DATA_DIR_TO_HOST,
> -				      (7<<1) | (1<<4), 1,
> -				      "READ(6)");
> +		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)
>  			reply = do_read(common);
>  		break;
>  
>  	case SC_READ_10:
>  		common->data_size_from_cmnd =
> -				get_unaligned_be16(&common->cmnd[7]) << 9;
> -		reply = check_command(common, 10, DATA_DIR_TO_HOST,
> -				      (1<<1) | (0xf<<2) | (3<<7), 1,
> -				      "READ(10)");
> +				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)
>  			reply = do_read(common);
>  		break;
>  
>  	case SC_READ_12:
>  		common->data_size_from_cmnd =
> -				get_unaligned_be32(&common->cmnd[6]) << 9;
> -		reply = check_command(common, 12, DATA_DIR_TO_HOST,
> -				      (1<<1) | (0xf<<2) | (0xf<<6), 1,
> -				      "READ(12)");
> +				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)
>  			reply = do_read(common);
>  		break;
> @@ -1983,30 +1994,30 @@ static int do_scsi_command(struct fsg_common *common)
>  
>  	case SC_WRITE_6:
>  		i = common->cmnd[4];
> -		common->data_size_from_cmnd = (i == 0 ? 256 : i) << 9;
> -		reply = check_command(common, 6, DATA_DIR_FROM_HOST,
> -				      (7<<1) | (1<<4), 1,
> -				      "WRITE(6)");
> +		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)
>  			reply = do_write(common);
>  		break;
>  
>  	case SC_WRITE_10:
>  		common->data_size_from_cmnd =
> -				get_unaligned_be16(&common->cmnd[7]) << 9;
> -		reply = check_command(common, 10, DATA_DIR_FROM_HOST,
> -				      (1<<1) | (0xf<<2) | (3<<7), 1,
> -				      "WRITE(10)");
> +				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)
>  			reply = do_write(common);
>  		break;
>  
>  	case SC_WRITE_12:
>  		common->data_size_from_cmnd =
> -				get_unaligned_be32(&common->cmnd[6]) << 9;
> -		reply = check_command(common, 12, DATA_DIR_FROM_HOST,
> -				      (1<<1) | (0xf<<2) | (0xf<<6), 1,
> -				      "WRITE(12)");
> +				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)
>  			reply = do_write(common);
>  		break;
> @@ -2497,7 +2508,7 @@ static struct fsg_common *fsg_common_init(struct fsg_common *common,
>  	for (i = 0; i < nluns; i++) {
>  		common->luns[i].removable = 1;
>  
> -		rc = fsg_lun_open(&common->luns[i], ums[i].num_sectors, "");
> +		rc = fsg_lun_open(&common->luns[i], ums[i].num_sectors, ums->block_dev.blksz, "");
>  		if (rc)
>  			goto error_luns;
>  	}
> diff --git a/drivers/usb/gadget/storage_common.c b/drivers/usb/gadget/storage_common.c
> index 5674e8fe4940..97dc6b6f729c 100644
> --- a/drivers/usb/gadget/storage_common.c
> +++ b/drivers/usb/gadget/storage_common.c
> @@ -269,6 +269,7 @@ struct device_attribute { int i; };
>  #define ETOOSMALL	525
>  
>  #include <log.h>
> +#include <linux/log2.h>
>  #include <usb_mass_storage.h>
>  #include <dm/device_compat.h>
>  
> @@ -290,6 +291,8 @@ struct fsg_lun {
>  	u32		sense_data;
>  	u32		sense_data_info;
>  	u32		unit_attention_data;
> +	unsigned int	blkbits;
> +	unsigned int	blksize; /* logical block size of bound block device */
>  
>  	struct device	dev;
>  };
> @@ -566,7 +569,7 @@ static struct usb_gadget_strings	fsg_stringtab = {
>   */
>  
>  static int fsg_lun_open(struct fsg_lun *curlun, unsigned int num_sectors,
> -			const char *filename)
> +			unsigned int sector_size, const char *filename)
>  {
>  	int				ro;
>  
> @@ -574,9 +577,12 @@ static int fsg_lun_open(struct fsg_lun *curlun, unsigned int num_sectors,
>  	ro = curlun->initially_ro;
>  
>  	curlun->ro = ro;
> -	curlun->file_length = num_sectors << 9;
> +	curlun->file_length = num_sectors * sector_size;
>  	curlun->num_sectors = num_sectors;
> -	debug("open backing file: %s\n", filename);
> +	curlun->blksize = sector_size;
> +	curlun->blkbits = order_base_2(sector_size >> 9) + 9;
> +	debug("blksize: %u\n", sector_size);
> +	debug("open backing file: '%s'\n", filename);
>  
>  	return 0;
>  }
> diff --git a/include/usb_mass_storage.h b/include/usb_mass_storage.h
> index 83ab93b530d7..6d83d93cad7f 100644
> --- a/include/usb_mass_storage.h
> +++ b/include/usb_mass_storage.h
> @@ -7,7 +7,6 @@
>  #ifndef __USB_MASS_STORAGE_H__
>  #define __USB_MASS_STORAGE_H__
>  
> -#define SECTOR_SIZE		0x200
>  #include <part.h>
>  #include <linux/usb/composite.h>
>  
>
> -- 
> 2.43.0
diff mbox series

Patch

diff --git a/cmd/usb_mass_storage.c b/cmd/usb_mass_storage.c
index a8ddeb494628..751701fe73af 100644
--- a/cmd/usb_mass_storage.c
+++ b/cmd/usb_mass_storage.c
@@ -88,10 +88,6 @@  static int ums_init(const char *devtype, const char *devnums_part_str)
 		if (!strchr(devnum_part_str, ':'))
 			partnum = 0;
 
-		/* f_mass_storage.c assumes SECTOR_SIZE sectors */
-		if (block_dev->blksz != SECTOR_SIZE)
-			goto cleanup;
-
 		ums_new = realloc(ums, (ums_count + 1) * sizeof(*ums));
 		if (!ums_new)
 			goto cleanup;
diff --git a/drivers/usb/gadget/f_mass_storage.c b/drivers/usb/gadget/f_mass_storage.c
index c725aed3f626..d880928044f4 100644
--- a/drivers/usb/gadget/f_mass_storage.c
+++ b/drivers/usb/gadget/f_mass_storage.c
@@ -724,12 +724,13 @@  static int do_read(struct fsg_common *common)
 		curlun->sense_data = SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
 		return -EINVAL;
 	}
-	file_offset = ((loff_t) lba) << 9;
+	file_offset = ((loff_t)lba) << curlun->blkbits;
 
 	/* Carry out the file reads */
 	amount_left = common->data_size_from_cmnd;
-	if (unlikely(amount_left == 0))
+	if (unlikely(amount_left == 0)) {
 		return -EIO;		/* No default reply */
+	}
 
 	for (;;) {
 
@@ -768,13 +769,13 @@  static int do_read(struct fsg_common *common)
 
 		/* Perform the read */
 		rc = ums[common->lun].read_sector(&ums[common->lun],
-				      file_offset / SECTOR_SIZE,
-				      amount / SECTOR_SIZE,
+				      file_offset / curlun->blksize,
+				      amount / curlun->blksize,
 				      (char __user *)bh->buf);
 		if (!rc)
 			return -EIO;
 
-		nread = rc * SECTOR_SIZE;
+		nread = rc * curlun->blksize;
 
 		VLDBG(curlun, "file read %u @ %llu -> %d\n", amount,
 				(unsigned long long) file_offset,
@@ -787,7 +788,7 @@  static int do_read(struct fsg_common *common)
 		} else if (nread < amount) {
 			LDBG(curlun, "partial file read: %d/%u\n",
 					(int) nread, amount);
-			nread -= (nread & 511);	/* Round down to a block */
+			nread -= (nread & (curlun->blksize - 1));	/* Round down to a block */
 		}
 		file_offset  += nread;
 		amount_left  -= nread;
@@ -861,7 +862,7 @@  static int do_write(struct fsg_common *common)
 
 	/* Carry out the file writes */
 	get_some_more = 1;
-	file_offset = usb_offset = ((loff_t) lba) << 9;
+	file_offset = usb_offset = ((loff_t)lba) << curlun->blkbits;
 	amount_left_to_req = common->data_size_from_cmnd;
 	amount_left_to_write = common->data_size_from_cmnd;
 
@@ -893,7 +894,7 @@  static int do_write(struct fsg_common *common)
 				curlun->info_valid = 1;
 				continue;
 			}
-			amount -= (amount & 511);
+			amount -= (amount & (curlun->blksize - 1));
 			if (amount == 0) {
 
 				/* Why were we were asked to transfer a
@@ -942,12 +943,12 @@  static int do_write(struct fsg_common *common)
 
 			/* Perform the write */
 			rc = ums[common->lun].write_sector(&ums[common->lun],
-					       file_offset / SECTOR_SIZE,
-					       amount / SECTOR_SIZE,
+					       file_offset / curlun->blksize,
+					       amount / curlun->blksize,
 					       (char __user *)bh->buf);
 			if (!rc)
 				return -EIO;
-			nwritten = rc * SECTOR_SIZE;
+			nwritten = rc * curlun->blksize;
 
 			VLDBG(curlun, "file write %u @ %llu -> %d\n", amount,
 					(unsigned long long) file_offset,
@@ -960,7 +961,7 @@  static int do_write(struct fsg_common *common)
 			} else if (nwritten < amount) {
 				LDBG(curlun, "partial file write: %d/%u\n",
 						(int) nwritten, amount);
-				nwritten -= (nwritten & 511);
+				nwritten -= (nwritten & (curlun->blksize - 1));
 				/* Round down to a block */
 			}
 			file_offset += nwritten;
@@ -1034,8 +1035,8 @@  static int do_verify(struct fsg_common *common)
 		return -EIO;		/* No default reply */
 
 	/* Prepare to carry out the file verify */
-	amount_left = verification_length << 9;
-	file_offset = ((loff_t) lba) << 9;
+	amount_left = verification_length << curlun->blkbits;
+	file_offset = ((loff_t) lba) << curlun->blkbits;
 
 	/* Write out all the dirty buffers before invalidating them */
 
@@ -1058,12 +1059,12 @@  static int do_verify(struct fsg_common *common)
 
 		/* Perform the read */
 		rc = ums[common->lun].read_sector(&ums[common->lun],
-				      file_offset / SECTOR_SIZE,
-				      amount / SECTOR_SIZE,
+				      file_offset / curlun->blksize,
+				      amount / curlun->blksize,
 				      (char __user *)bh->buf);
 		if (!rc)
 			return -EIO;
-		nread = rc * SECTOR_SIZE;
+		nread = rc * curlun->blksize;
 
 		VLDBG(curlun, "file read %u @ %llu -> %d\n", amount,
 				(unsigned long long) file_offset,
@@ -1075,7 +1076,7 @@  static int do_verify(struct fsg_common *common)
 		} else if (nread < amount) {
 			LDBG(curlun, "partial file verify: %d/%u\n",
 					(int) nread, amount);
-			nread -= (nread & 511);	/* Round down to a sector */
+			nread -= (nread & (curlun->blksize - 1));	/* Round down to a sector */
 		}
 		if (nread == 0) {
 			curlun->sense_data = SS_UNRECOVERED_READ_ERROR;
@@ -1183,7 +1184,7 @@  static int do_read_capacity(struct fsg_common *common, struct fsg_buffhd *bh)
 
 	put_unaligned_be32(curlun->num_sectors - 1, &buf[0]);
 						/* Max logical block */
-	put_unaligned_be32(512, &buf[4]);	/* Block length */
+	put_unaligned_be32(curlun->blksize, &buf[4]);	/* Block length */
 	return 8;
 }
 
@@ -1370,7 +1371,7 @@  static int do_read_format_capacities(struct fsg_common *common,
 
 	put_unaligned_be32(curlun->num_sectors, &buf[0]);
 						/* Number of blocks */
-	put_unaligned_be32(512, &buf[4]);	/* Block length */
+	put_unaligned_be32(curlun->blksize, &buf[4]);	/* Block length */
 	buf[4] = 0x02;				/* Current capacity */
 	return 12;
 }
@@ -1781,6 +1782,16 @@  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)
+{
+	common->data_size_from_cmnd <<= common->luns[common->lun].blkbits;
+	return check_command(common, cmnd_size, data_dir,
+			mask, needs_medium, name);
+}
+
 
 static int do_scsi_command(struct fsg_common *common)
 {
@@ -1865,30 +1876,30 @@  static int do_scsi_command(struct fsg_common *common)
 
 	case SC_READ_6:
 		i = common->cmnd[4];
-		common->data_size_from_cmnd = (i == 0 ? 256 : i) << 9;
-		reply = check_command(common, 6, DATA_DIR_TO_HOST,
-				      (7<<1) | (1<<4), 1,
-				      "READ(6)");
+		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)
 			reply = do_read(common);
 		break;
 
 	case SC_READ_10:
 		common->data_size_from_cmnd =
-				get_unaligned_be16(&common->cmnd[7]) << 9;
-		reply = check_command(common, 10, DATA_DIR_TO_HOST,
-				      (1<<1) | (0xf<<2) | (3<<7), 1,
-				      "READ(10)");
+				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)
 			reply = do_read(common);
 		break;
 
 	case SC_READ_12:
 		common->data_size_from_cmnd =
-				get_unaligned_be32(&common->cmnd[6]) << 9;
-		reply = check_command(common, 12, DATA_DIR_TO_HOST,
-				      (1<<1) | (0xf<<2) | (0xf<<6), 1,
-				      "READ(12)");
+				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)
 			reply = do_read(common);
 		break;
@@ -1983,30 +1994,30 @@  static int do_scsi_command(struct fsg_common *common)
 
 	case SC_WRITE_6:
 		i = common->cmnd[4];
-		common->data_size_from_cmnd = (i == 0 ? 256 : i) << 9;
-		reply = check_command(common, 6, DATA_DIR_FROM_HOST,
-				      (7<<1) | (1<<4), 1,
-				      "WRITE(6)");
+		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)
 			reply = do_write(common);
 		break;
 
 	case SC_WRITE_10:
 		common->data_size_from_cmnd =
-				get_unaligned_be16(&common->cmnd[7]) << 9;
-		reply = check_command(common, 10, DATA_DIR_FROM_HOST,
-				      (1<<1) | (0xf<<2) | (3<<7), 1,
-				      "WRITE(10)");
+				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)
 			reply = do_write(common);
 		break;
 
 	case SC_WRITE_12:
 		common->data_size_from_cmnd =
-				get_unaligned_be32(&common->cmnd[6]) << 9;
-		reply = check_command(common, 12, DATA_DIR_FROM_HOST,
-				      (1<<1) | (0xf<<2) | (0xf<<6), 1,
-				      "WRITE(12)");
+				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)
 			reply = do_write(common);
 		break;
@@ -2497,7 +2508,7 @@  static struct fsg_common *fsg_common_init(struct fsg_common *common,
 	for (i = 0; i < nluns; i++) {
 		common->luns[i].removable = 1;
 
-		rc = fsg_lun_open(&common->luns[i], ums[i].num_sectors, "");
+		rc = fsg_lun_open(&common->luns[i], ums[i].num_sectors, ums->block_dev.blksz, "");
 		if (rc)
 			goto error_luns;
 	}
diff --git a/drivers/usb/gadget/storage_common.c b/drivers/usb/gadget/storage_common.c
index 5674e8fe4940..97dc6b6f729c 100644
--- a/drivers/usb/gadget/storage_common.c
+++ b/drivers/usb/gadget/storage_common.c
@@ -269,6 +269,7 @@  struct device_attribute { int i; };
 #define ETOOSMALL	525
 
 #include <log.h>
+#include <linux/log2.h>
 #include <usb_mass_storage.h>
 #include <dm/device_compat.h>
 
@@ -290,6 +291,8 @@  struct fsg_lun {
 	u32		sense_data;
 	u32		sense_data_info;
 	u32		unit_attention_data;
+	unsigned int	blkbits;
+	unsigned int	blksize; /* logical block size of bound block device */
 
 	struct device	dev;
 };
@@ -566,7 +569,7 @@  static struct usb_gadget_strings	fsg_stringtab = {
  */
 
 static int fsg_lun_open(struct fsg_lun *curlun, unsigned int num_sectors,
-			const char *filename)
+			unsigned int sector_size, const char *filename)
 {
 	int				ro;
 
@@ -574,9 +577,12 @@  static int fsg_lun_open(struct fsg_lun *curlun, unsigned int num_sectors,
 	ro = curlun->initially_ro;
 
 	curlun->ro = ro;
-	curlun->file_length = num_sectors << 9;
+	curlun->file_length = num_sectors * sector_size;
 	curlun->num_sectors = num_sectors;
-	debug("open backing file: %s\n", filename);
+	curlun->blksize = sector_size;
+	curlun->blkbits = order_base_2(sector_size >> 9) + 9;
+	debug("blksize: %u\n", sector_size);
+	debug("open backing file: '%s'\n", filename);
 
 	return 0;
 }
diff --git a/include/usb_mass_storage.h b/include/usb_mass_storage.h
index 83ab93b530d7..6d83d93cad7f 100644
--- a/include/usb_mass_storage.h
+++ b/include/usb_mass_storage.h
@@ -7,7 +7,6 @@ 
 #ifndef __USB_MASS_STORAGE_H__
 #define __USB_MASS_STORAGE_H__
 
-#define SECTOR_SIZE		0x200
 #include <part.h>
 #include <linux/usb/composite.h>