From 304fa0aa445384e5e681a54abf413850591cec10 Mon Sep 17 00:00:00 2001 From: Caleb Connolly Date: Wed, 20 Mar 2024 14:30:50 +0000 Subject: [PATCH] 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 Reviewed-by: Neil Armstrong Signed-off-by: Caleb Connolly 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 --- cmd/usb_mass_storage.c | 4 -- drivers/usb/gadget/f_mass_storage.c | 102 ++++++++++++++++------------ drivers/usb/gadget/storage_common.c | 12 +++- include/usb_mass_storage.h | 1 - 4 files changed, 66 insertions(+), 53 deletions(-) diff --git a/cmd/usb_mass_storage.c b/cmd/usb_mass_storage.c index a8ddeb4946..751701fe73 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 c725aed3f6..ef90c7ec7f 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 +#include #include #include #include @@ -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 5674e8fe49..97dc6b6f72 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 +#include #include #include @@ -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 83ab93b530..6d83d93cad 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 #include -- 2.39.5