diff --git a/common/cmd_usb_mass_storage.c b/common/cmd_usb_mass_storage.c index f4bafec..0415591 100644 --- a/common/cmd_usb_mass_storage.c +++ b/common/cmd_usb_mass_storage.c @@ -2,6 +2,8 @@ * Copyright (C) 2011 Samsung Electronics * Lukasz Majewski * + * Copyright (c) 2015, NVIDIA CORPORATION. All rights reserved. + * * SPDX-License-Identifier: GPL-2.0+ */ @@ -17,7 +19,7 @@ static int ums_read_sector(struct ums *ums_dev, ulong start, lbaint_t blkcnt, void *buf) { - block_dev_desc_t *block_dev = ums_dev->block_dev; + block_dev_desc_t *block_dev = &ums_dev->block_dev; lbaint_t blkstart = start + ums_dev->start_sector; return block_dev->block_read(block_dev, blkstart, blkcnt, buf); @@ -26,39 +28,98 @@ static int ums_read_sector(struct ums *ums_dev, static int ums_write_sector(struct ums *ums_dev, ulong start, lbaint_t blkcnt, const void *buf) { - block_dev_desc_t *block_dev = ums_dev->block_dev; + block_dev_desc_t *block_dev = &ums_dev->block_dev; lbaint_t blkstart = start + ums_dev->start_sector; return block_dev->block_write(block_dev, blkstart, blkcnt, buf); } -static struct ums ums_dev = { - .read_sector = ums_read_sector, - .write_sector = ums_write_sector, - .name = "UMS disk", -}; +static struct ums *ums; +static int ums_count; + +static void ums_fini(void) +{ + int i; + + for (i = 0; i < ums_count; i++) + free((void *)ums[i].name); + free(ums); + ums = 0; + ums_count = 0; +} + +#define UMS_NAME_LEN 16 -struct ums *ums_init(const char *devtype, const char *devnum) +static int ums_init(const char *devtype, const char *devnums) { + char *s, *t, *devnum, *name; block_dev_desc_t *block_dev; int ret; + struct ums *ums_new; - ret = get_device(devtype, devnum, &block_dev); - if (ret < 0) - return NULL; + s = strdup(devnums); + if (!s) + return -1; + + t = s; + ums_count = 0; + + for (;;) { + devnum = strsep(&t, ","); + if (!devnum) + break; + + ret = get_device(devtype, devnum, &block_dev); + if (ret < 0) + goto cleanup; + + /* f_mass_storage.c assumes SECTOR_SIZE sectors */ + if (block_dev->blksz != SECTOR_SIZE) { + ret = -1; + goto cleanup; + } - /* f_mass_storage.c assumes SECTOR_SIZE sectors */ - if (block_dev->blksz != SECTOR_SIZE) - return NULL; + ums_new = realloc(ums, (ums_count + 1) * sizeof(*ums)); + if (!ums_new) { + ret = -1; + goto cleanup; + } + ums = ums_new; + + ums[ums_count].read_sector = ums_read_sector; + ums[ums_count].write_sector = ums_write_sector; + ums[ums_count].start_sector = 0; + ums[ums_count].num_sectors = block_dev->lba; + name = malloc(UMS_NAME_LEN); + if (!name) { + ret = -1; + goto cleanup; + } + snprintf(name, UMS_NAME_LEN, "UMS disk %d", ums_count); + ums[ums_count].name = name; + ums[ums_count].block_dev = *block_dev; + + printf("UMS: LUN %d, dev %d, hwpart %d, sector %#x, count %#x\n", + ums_count, ums[ums_count].block_dev.dev, + ums[ums_count].block_dev.hwpart, + ums[ums_count].start_sector, + ums[ums_count].num_sectors); + + ums_count++; + } + + if (!ums_count) + ret = -1; + else + ret = 0; - ums_dev.block_dev = block_dev; - ums_dev.start_sector = 0; - ums_dev.num_sectors = block_dev->lba; +cleanup: + free(s); - printf("UMS: disk start sector: %#x, count: %#x\n", - ums_dev.start_sector, ums_dev.num_sectors); + if (ret < 0) + ums_fini(); - return &ums_dev; + return ret; } int do_usb_mass_storage(cmd_tbl_t *cmdtp, int flag, @@ -67,7 +128,6 @@ int do_usb_mass_storage(cmd_tbl_t *cmdtp, int flag, const char *usb_controller; const char *devtype; const char *devnum; - struct ums *ums; unsigned int controller_index; int rc; int cable_ready_timeout __maybe_unused; @@ -84,27 +144,30 @@ int do_usb_mass_storage(cmd_tbl_t *cmdtp, int flag, devnum = argv[2]; } - ums = ums_init(devtype, devnum); - if (!ums) + rc = ums_init(devtype, devnum); + if (rc < 0) return CMD_RET_FAILURE; controller_index = (unsigned int)(simple_strtoul( usb_controller, NULL, 0)); if (board_usb_init(controller_index, USB_INIT_DEVICE)) { error("Couldn't init USB controller."); - return CMD_RET_FAILURE; + rc = CMD_RET_FAILURE; + goto cleanup_ums_init; } - rc = fsg_init(ums); + rc = fsg_init(ums, ums_count); if (rc) { error("fsg_init failed"); - return CMD_RET_FAILURE; + rc = CMD_RET_FAILURE; + goto cleanup_board; } rc = g_dnl_register("usb_dnl_ums"); if (rc) { error("g_dnl_register failed"); - return CMD_RET_FAILURE; + rc = CMD_RET_FAILURE; + goto cleanup_board; } /* Timeout unit: seconds */ @@ -120,12 +183,14 @@ int do_usb_mass_storage(cmd_tbl_t *cmdtp, int flag, while (!g_dnl_board_usb_cable_connected()) { if (ctrlc()) { puts("\rCTRL+C - Operation aborted.\n"); - goto exit; + rc = CMD_RET_SUCCESS; + goto cleanup_register; } if (!cable_ready_timeout) { puts("\rUSB cable not detected.\n" \ "Command exit.\n"); - goto exit; + rc = CMD_RET_SUCCESS; + goto cleanup_register; } printf("\rAuto exit in: %.2d s.", cable_ready_timeout); @@ -148,13 +213,19 @@ int do_usb_mass_storage(cmd_tbl_t *cmdtp, int flag, if (rc == -EPIPE) printf("\rCTRL+C - Operation aborted\n"); - goto exit; + rc = CMD_RET_SUCCESS; + goto cleanup_register; } } -exit: + +cleanup_register: g_dnl_unregister(); +cleanup_board: board_usb_cleanup(controller_index, USB_INIT_DEVICE); - return CMD_RET_SUCCESS; +cleanup_ums_init: + ums_fini(); + + return rc; } U_BOOT_CMD(ums, 4, 1, do_usb_mass_storage, diff --git a/drivers/usb/gadget/f_mass_storage.c b/drivers/usb/gadget/f_mass_storage.c index ec1f23a..1ecb92a 100644 --- a/drivers/usb/gadget/f_mass_storage.c +++ b/drivers/usb/gadget/f_mass_storage.c @@ -444,8 +444,9 @@ static void set_bulk_out_req_length(struct fsg_common *common, /*-------------------------------------------------------------------------*/ -struct ums *ums; -struct fsg_common *the_fsg_common; +static struct ums *ums; +static int ums_count; +static struct fsg_common *the_fsg_common; static int fsg_set_halt(struct fsg_dev *fsg, struct usb_ep *ep) { @@ -772,7 +773,7 @@ static int do_read(struct fsg_common *common) } /* Perform the read */ - rc = ums->read_sector(ums, + rc = ums[common->lun].read_sector(&ums[common->lun], file_offset / SECTOR_SIZE, amount / SECTOR_SIZE, (char __user *)bh->buf); @@ -946,7 +947,7 @@ static int do_write(struct fsg_common *common) amount = bh->outreq->actual; /* Perform the write */ - rc = ums->write_sector(ums, + rc = ums[common->lun].write_sector(&ums[common->lun], file_offset / SECTOR_SIZE, amount / SECTOR_SIZE, (char __user *)bh->buf); @@ -1062,7 +1063,7 @@ static int do_verify(struct fsg_common *common) } /* Perform the read */ - rc = ums->read_sector(ums, + rc = ums[common->lun].read_sector(&ums[common->lun], file_offset / SECTOR_SIZE, amount / SECTOR_SIZE, (char __user *)bh->buf); @@ -1117,7 +1118,7 @@ static int do_inquiry(struct fsg_common *common, struct fsg_buffhd *bh) buf[4] = 31; /* Additional length */ /* No special options */ sprintf((char *) (buf + 8), "%-8s%-16s%04x", (char*) vendor_id , - ums->name, (u16) 0xffff); + ums[common->lun].name, (u16) 0xffff); return 36; } @@ -2456,7 +2457,7 @@ static struct fsg_common *fsg_common_init(struct fsg_common *common, int nluns, i, rc; /* Find out how many LUNs there should be */ - nluns = 1; + nluns = ums_count; if (nluns < 1 || nluns > FSG_MAX_LUNS) { printf("invalid number of LUNs: %u\n", nluns); return ERR_PTR(-EINVAL); @@ -2501,7 +2502,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], ""); + rc = fsg_lun_open(&common->luns[i], ums[i].num_sectors, ""); if (rc) goto error_luns; } @@ -2775,9 +2776,10 @@ int fsg_add(struct usb_configuration *c) return fsg_bind_config(c->cdev, c, fsg_common); } -int fsg_init(struct ums *ums_dev) +int fsg_init(struct ums *ums_devs, int count) { - ums = ums_dev; + ums = ums_devs; + ums_count = count; return 0; } diff --git a/drivers/usb/gadget/storage_common.c b/drivers/usb/gadget/storage_common.c index b55e40b..b6df130 100644 --- a/drivers/usb/gadget/storage_common.c +++ b/drivers/usb/gadget/storage_common.c @@ -564,7 +564,8 @@ static struct usb_gadget_strings fsg_stringtab = { * the caller must own fsg->filesem for writing. */ -static int fsg_lun_open(struct fsg_lun *curlun, const char *filename) +static int fsg_lun_open(struct fsg_lun *curlun, unsigned int num_sectors, + const char *filename) { int ro; @@ -572,8 +573,8 @@ static int fsg_lun_open(struct fsg_lun *curlun, const char *filename) ro = curlun->initially_ro; curlun->ro = ro; - curlun->file_length = ums->num_sectors << 9; - curlun->num_sectors = ums->num_sectors; + curlun->file_length = num_sectors << 9; + curlun->num_sectors = num_sectors; debug("open backing file: %s\n", filename); return 0; diff --git a/include/usb_mass_storage.h b/include/usb_mass_storage.h index 69b80cd..5804b70 100644 --- a/include/usb_mass_storage.h +++ b/include/usb_mass_storage.h @@ -23,12 +23,10 @@ struct ums { unsigned int start_sector; unsigned int num_sectors; const char *name; - block_dev_desc_t *block_dev; + block_dev_desc_t block_dev; }; -extern struct ums *ums; - -int fsg_init(struct ums *); +int fsg_init(struct ums *ums_devs, int count); void fsg_cleanup(void); int fsg_main_thread(void *); int fsg_add(struct usb_configuration *c);