usb: gadget: UMS: support multiple sector sizes

UFS storage often uses a 4096-byte sector size, add support for dynamic
sector sizes based loosely on the Linux implementation.

Support for dynamic sector sizes changes the types used in some
divisions, resulting in the compiler attempting to use
libgcc helpers (__aeabi_ldivmod).
Replace these divisions with calls to lldiv() to handle this correctly.

Reviewed-by: Mattijs Korpershoek <mkorpershoek@baylibre.com>
Reviewed-by: Neil Armstrong <neil.armstrong@linaro.org>
Signed-off-by: Caleb Connolly <caleb.connolly@linaro.org>
Link: https://lore.kernel.org/r/20240320-b4-qcom-usb-v4-4-41be480172e1@linaro.org
[mkorpershoek: squashed the lldiv() fix from caleb]
Signed-off-by: Mattijs Korpershoek <mkorpershoek@baylibre.com>

diff --git a/cmd/usb_mass_storage.c b/cmd/usb_mass_storage.c
index a8ddeb4946289e5769a72cdf92a58660307335e4..751701fe73aff2c579e022bbbc2adbaad9859f11 100644 (file)
--- 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 c725aed3f626566b03bd0e3720bbe36250e85594..ef90c7ec7fb5a274e40eae2670343862d3d09eda 100644 (file)
--- a/drivers/usb/gadget/f_mass_storage.c
+++ b/drivers/usb/gadget/f_mass_storage.c
@@ -240,6 +240,7 @@
 /* #define DUMP_MSGS */
 
 #include <config.h>
+#include <div64.h>
 #include <hexdump.h>
 #include <log.h>
 #include <malloc.h>
@@ -724,12 +725,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 +770,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,
+                                     lldiv(file_offset, curlun->blksize),
+                                     lldiv(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 +789,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 +863,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 +895,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 +944,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,
+                                              lldiv(file_offset, curlun->blksize),
+                                              lldiv(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 +962,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 +1036,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 +1060,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,
+                                     lldiv(file_offset, curlun->blksize),
+                                     lldiv(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 +1077,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 +1185,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 +1372,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 +1783,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 +1877,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 +1995,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 +2509,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 5674e8fe494050d30af2ad16813ee3b309d42906..97dc6b6f729c25b284b1ac9c27fc5e58a194e62c 100644 (file)
--- 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 83ab93b530d7448e704c9ac5bdf16aa6f354c5f2..6d83d93cad7fb2c21d91cf4cc482173e65788c53 100644 (file)
--- 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>