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.
Signed-off-by: Caleb Connolly <caleb.connolly@linaro.org>
diff --git a/cmd/usb_mass_storage.c b/cmd/usb_mass_storage.c
index a8ddeb4..799a2a6 100644
--- a/cmd/usb_mass_storage.c
+++ b/cmd/usb_mass_storage.c
@@ -10,6 +10,7 @@
#include <blk.h>
#include <command.h>
#include <console.h>
+#include <dm.h>
#include <errno.h>
#include <g_dnl.h>
#include <malloc.h>
@@ -26,6 +27,8 @@
struct blk_desc *block_dev = &ums_dev->block_dev;
lbaint_t blkstart = start + ums_dev->start_sector;
+ //printf("%s: blk %s read %lu bytes @ %#lx\n", __func__, block_dev->bdev->name, blkcnt, blkstart);
+
return blk_dread(block_dev, blkstart, blkcnt, buf);
}
@@ -88,10 +91,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 1d17331..ee1c192 100644
--- a/drivers/usb/gadget/f_mass_storage.c
+++ b/drivers/usb/gadget/f_mass_storage.c
@@ -719,12 +719,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 (;;) {
@@ -763,13 +764,15 @@
/* 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)
+ if (!rc) {
+ printf("read failed!\n");
return -EIO;
+ }
- nread = rc * SECTOR_SIZE;
+ nread = rc * curlun->blksize;
VLDBG(curlun, "file read %u @ %llu -> %d\n", amount,
(unsigned long long) file_offset,
@@ -782,7 +785,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;
@@ -856,7 +859,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;
@@ -888,7 +891,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
@@ -937,12 +940,12 @@
/* 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,
@@ -955,7 +958,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;
@@ -1029,8 +1032,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 */
@@ -1053,12 +1056,12 @@
/* 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,
@@ -1070,7 +1073,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;
@@ -1178,7 +1181,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;
}
@@ -1363,7 +1366,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;
}
@@ -1774,6 +1777,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)
{
@@ -1858,8 +1871,8 @@
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,
+ 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)
@@ -1868,8 +1881,8 @@
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,
+ 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)
@@ -1878,8 +1891,8 @@
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,
+ 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)
@@ -1976,8 +1989,8 @@
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,
+ 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)
@@ -1986,8 +1999,8 @@
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,
+ 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)
@@ -1996,8 +2009,8 @@
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,
+ 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)
@@ -2490,7 +2503,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..87075df 100644
--- a/drivers/usb/gadget/storage_common.c
+++ b/drivers/usb/gadget/storage_common.c
@@ -49,7 +49,6 @@
/* #include <asm/unaligned.h> */
-
/*
* Thanks to NetChip Technologies for donating this product ID.
*
@@ -269,6 +268,7 @@
#define ETOOSMALL 525
#include <log.h>
+#include <linux/log2.h>
#include <usb_mass_storage.h>
#include <dm/device_compat.h>
@@ -290,6 +290,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 +568,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 +576,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>