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 a8ddeb4..751701f 100644
--- a/cmd/usb_mass_storage.c
+++ b/cmd/usb_mass_storage.c
@@ -88,10 +88,6 @@
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 c725aed..ef90c7e 100644
--- 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 @@
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 @@
/* 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 @@
} 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 @@
/* 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 @@
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 @@
/* 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 @@
} 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 @@
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 @@
/* 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 @@
} 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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
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 5674e8f..97dc6b6 100644
--- a/drivers/usb/gadget/storage_common.c
+++ b/drivers/usb/gadget/storage_common.c
@@ -269,6 +269,7 @@
#define ETOOSMALL 525
#include <log.h>
+#include <linux/log2.h>
#include <usb_mass_storage.h>
#include <dm/device_compat.h>
@@ -290,6 +291,8 @@
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 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 @@
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 83ab93b..6d83d93 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>