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.conno...@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(-)

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

Reply via email to