diff --git a/drivers/usb/gadget/f_mass_storage.c b/drivers/usb/gadget/f_mass_storage.c index 0843301..17a85e6 100644 --- a/drivers/usb/gadget/f_mass_storage.c +++ b/drivers/usb/gadget/f_mass_storage.c @@ -299,6 +299,9 @@ /*------------------------------------------------------------------------*/ +/* Not defined in scsi headers */ +#define READ_CD 0xbe + #define FSG_DRIVER_DESC "Mass Storage Function" #define FSG_DRIVER_VERSION "2009/09/11" @@ -737,6 +740,30 @@ static int sleep_thread(struct fsg_common *common) return rc; } +/*-------------------------------------------------------------------------*/ + +/* Returns the output block size given the command */ +static int get_output_block_size(struct fsg_common *common) +{ + int size; + + if(common->curlun) { + if(common->cmnd[0] == READ_CD) { + if(common->cmnd[9] == 0xf8) + size = 2352; + else if(common->cmnd[9] == 0x10) + size = 1 << common->curlun->blkbits; + else + size = 0; + } + else + size = 1 << common->curlun->blkbits; + } + else + size = 1; /* Can we get here? */ + + return size; +} /*-------------------------------------------------------------------------*/ @@ -880,6 +907,173 @@ static int do_read(struct fsg_common *common) return -EIO; /* No default reply */ } +/*-------------------------------------------------------------------------*/ + +static int read_cd_sector(struct fsg_common *common, char *sector_buf, int sector_size, int block_size, loff_t *file_offset) +{ + static const char header[12] = { + 0x0, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0 + }; + struct fsg_lun *curlun = common->curlun; + int block,m,s,f; + int nread; + struct fsg_buffhd *bh = common->next_buffhd_to_fill; + int remainder, buf_remaining; + int rc; + + /* MSF is calculated based on 75 sectors per second */ + /* See MMC r3 page 282 */ + block = (*file_offset / block_size) + 150; + m = block / 4500; + s = (block - (m * 4500)) / 75; + f = block - (m * 4500) - (s * 75); + + memcpy(sector_buf, header, 12); + sector_buf[12] = m + 0xa0; + sector_buf[13] = s; + sector_buf[14] = f; + sector_buf[15] = 1; // Mode 1 CDROM sector + + /* We don't do CRC here - we assume the receiver doesn't either */ + nread = vfs_read(curlun->filp, + sector_buf + 16, + block_size, file_offset); + + + if(nread < block_size) { + return nread; + } + + buf_remaining = FSG_BUFLEN - bh->inreq->length; + if(buf_remaining >= sector_size) { + memcpy(bh->buf + bh->inreq->length, sector_buf, sector_size); + bh->inreq->length += sector_size; + bh->state = BUF_STATE_FULL; + common->residue -= sector_size; + } else { + remainder = sector_size - buf_remaining; + memcpy(bh->buf + bh->inreq->length, sector_buf, sector_size - remainder); + + common->residue -= sector_size; + bh->inreq->length = FSG_BUFLEN; + bh->state = BUF_STATE_FULL; + + /* Send this buffer and go read some more */ + bh->inreq->zero = 0; + if (!start_in_transfer(common, bh)) + /* Don't know what to do if common->fsg is NULL */ + return -EIO; + + common->next_buffhd_to_fill = bh->next; + + /* Wait for the next buffer to become available */ + bh = common->next_buffhd_to_fill; + while (bh->state != BUF_STATE_EMPTY) { + rc = sleep_thread(common); + if (rc) + return rc; + } + bh->inreq->length = 0; + + if(remainder > 0) + memcpy(bh->buf, sector_buf + sector_size - remainder, remainder); + + bh->inreq->length = remainder; + if(remainder > 0) + bh->state = BUF_STATE_FULL; + } + + return sector_size; +} + +static int do_read_cd(struct fsg_common *common) +{ + struct fsg_lun *curlun = common->curlun; + u32 lba; + struct fsg_buffhd *bh; + int rc; + u32 amount_left; + int sector_size, block_size; + loff_t file_offset; + ssize_t nread; + char *sector_buf; + + block_size = 2048; + sector_size = get_output_block_size(common); + /* We only support 2048 or 2352 byte sectors.. others exist but should be uncommon */ + /* Furhermore there's an assumption in this code that the base block size is 2048 */ + if(sector_size < 16) { + if (curlun) + curlun->sense_data = SS_INVALID_COMMAND; + return -EINVAL; + } + + /* In the 2048 case, just use the standard READ_10 command */ + if(sector_size == block_size) { + return do_read(common); + } + + /* + * Get the starting Logical Block Address and check that it's + * not too big. + */ + lba = get_unaligned_be32(&common->cmnd[2]); + + /* + * We allow DPO (Disable Page Out = don't save data in the + * cache) and FUA (Force Unit Access = don't read from the + * cache), but we don't implement them. + */ + if ((common->cmnd[1] & ~0x18) != 0) { + curlun->sense_data = SS_INVALID_FIELD_IN_CDB; + return -EINVAL; + } + + if (lba >= curlun->num_sectors) { + curlun->sense_data = SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE; + return -EINVAL; + } + + + file_offset = ((loff_t) lba) * block_size; + + /* Carry out the file reads */ + amount_left = common->data_size_from_cmnd; + if (unlikely(amount_left == 0)) + return -EIO; /* No default reply */ + + + /* Wait for the next buffer to become available */ + bh = common->next_buffhd_to_fill; + while (bh->state != BUF_STATE_EMPTY) { + rc = sleep_thread(common); + if (rc) + return rc; + } + bh->inreq->length = 0; + + sector_buf = kmalloc(sector_size, GFP_KERNEL); + while(amount_left >= sector_size) { + if (signal_pending(current)) + return -EINTR; + + nread = read_cd_sector(common, sector_buf, sector_size, block_size, &file_offset); + if(nread < 0) { + /* If an error occurred, report it and its position */ + curlun->sense_data = SS_UNRECOVERED_READ_ERROR; + curlun->sense_data_info = + file_offset >> curlun->blkbits; + curlun->info_valid = 1; + break; + } + + amount_left -= nread; + } + + kfree(sector_buf); + + return -EIO; /* No default reply */ +} /*-------------------------------------------------------------------------*/ @@ -1374,31 +1568,57 @@ static int do_read_header(struct fsg_common *common, struct fsg_buffhd *bh) return 8; } -static int do_read_toc(struct fsg_common *common, struct fsg_buffhd *bh) +static void add_toc_track(u8 *buf, u8 session, u8 flags, u8 point, u8 pmin, int msf) { - struct fsg_lun *curlun = common->curlun; - int msf = common->cmnd[1] & 0x02; - int start_track = common->cmnd[6]; - u8 *buf = (u8 *)bh->buf; - - if ((common->cmnd[1] & ~0x02) != 0 || /* Mask away MSF */ - start_track > 1) { - curlun->sense_data = SS_INVALID_FIELD_IN_CDB; - return -EINVAL; - } - - memset(buf, 0, 20); - buf[1] = (20-2); /* TOC data length */ - buf[2] = 1; /* First track number */ - buf[3] = 1; /* Last track number */ - buf[5] = 0x16; /* Data track, copying allowed */ - buf[6] = 0x01; /* Only track is number 1 */ - store_cdrom_address(&buf[8], msf, 0); + store_cdrom_address(&buf[3], msf, 0); + buf[0] = session; + buf[1] = flags; + buf[3] = point; + buf[8] = pmin; +} - buf[13] = 0x16; /* Lead-out track is data */ - buf[14] = 0xAA; /* Lead-out track number */ - store_cdrom_address(&buf[16], msf, curlun->num_sectors); - return 20; +static int do_read_toc(struct fsg_common *common, struct fsg_buffhd *bh) +{ + struct fsg_lun *curlun = common->curlun; + int msf = common->cmnd[1] & 0x02; + int version = common->cmnd[2] & 0xf; + u8 *buf = (u8 *)bh->buf; + + /* Some OSs - take a bow Apple - use the obsolete vendor specific + bits to specify the version */ + if(version == 0) { + version = common->cmnd[9] >> 6; + } + + if (version != 0 && version != 2) { /* Only support read TOC */ + curlun->sense_data = SS_INVALID_FIELD_IN_CDB; + return -EINVAL; + } + + if (version == 0) { + memset(buf, 0, 20); + buf[1] = (20-2); /* TOC data length */ + buf[2] = 1; /* First track number */ + buf[3] = 1; /* Last track number */ + buf[5] = 0x16; /* Data track, copying allowed */ + buf[6] = 0x01; /* Only track is number 1 */ + store_cdrom_address(&buf[8], msf, 0); + + buf[13] = 0x16; /* Lead-out track is data */ + buf[14] = 0xAA; /* Lead-out track number */ + store_cdrom_address(&buf[16], msf, curlun->num_sectors); + } else if(version == 2) { + memset(buf, 0, 48); + buf[1] = (48-2); /* TOC data length */ + buf[2] = 1; /* First track number */ + buf[3] = 1; /* Last track number */ + add_toc_track(buf+4, 1, 0x16, 0xa0, 1, msf); + add_toc_track(buf+15, 1, 0x16, 0xa1, 1, msf); + add_toc_track(buf+26, 1, 0x16, 0xa2, 0, msf); + store_cdrom_address(&buf[33], msf, curlun->num_sectors); + add_toc_track(buf+37, 1, 0x16, 0x01, 0, msf); + } + return buf[1]+2; } static int do_mode_sense(struct fsg_common *common, struct fsg_buffhd *bh) @@ -2131,7 +2351,21 @@ static int do_scsi_command(struct fsg_common *common) reply = do_read(common); break; - case READ_CAPACITY: + case READ_CD: + if (!common->curlun || !common->curlun->cdrom) + goto unknown_cmnd; + + common->data_size_from_cmnd = + get_unaligned_be24(&common->cmnd[6]); + reply = check_command_size_in_blocks(common, 12, + DATA_DIR_TO_HOST, + (1<<1) | (2<<1) | (0xf<<2) | (0x7<<6) | (3<<9), 1, + "READ CD"); + if (reply == 0) + reply = do_read_cd(common); + break; + + case READ_CAPACITY: common->data_size_from_cmnd = 8; reply = check_command(common, 10, DATA_DIR_TO_HOST, (0xf<<2) | (1<<8), 1, @@ -2158,7 +2392,7 @@ static int do_scsi_command(struct fsg_common *common) common->data_size_from_cmnd = get_unaligned_be16(&common->cmnd[7]); reply = check_command(common, 10, DATA_DIR_TO_HOST, - (7<<6) | (1<<1), 1, + (0xf<<6) | (1<<1), 1, "READ TOC"); if (reply == 0) reply = do_read_toc(common, bh); @@ -2786,6 +3020,41 @@ static int fsg_main_thread(void *common_) complete_and_exit(&common->thread_notifier, 0); } +/* CDROM Attribute */ +static ssize_t fsg_store_cdrom(struct device *dev, struct device_attribute *attr, + const char *buf, size_t count) +{ + ssize_t rc; + struct fsg_lun *curlun = fsg_lun_from_dev(dev); + struct rw_semaphore *filesem = dev_get_drvdata(dev); + unsigned cdrom; + + rc = kstrtouint(buf, 2, &cdrom); + if (rc) + return rc; + + /* + * Allow the cdrom status to change only while the + * backing file is closed. + */ + down_read(filesem); + if (fsg_lun_is_open(curlun)) { + rc = -EBUSY; + } else { + curlun->cdrom = cdrom; + rc = count; + } + up_read(filesem); + return rc; +} + +static ssize_t fsg_show_cdrom(struct device *dev, struct device_attribute *attr, + char *buf) +{ + struct fsg_lun *curlun = fsg_lun_from_dev(dev); + + return sprintf(buf, "%d\n", curlun->cdrom); +} /*************************** DEVICE ATTRIBUTES ***************************/ @@ -2793,6 +3062,7 @@ static int fsg_main_thread(void *common_) static DEVICE_ATTR(ro, 0644, fsg_show_ro, fsg_store_ro); static DEVICE_ATTR(nofua, 0644, fsg_show_nofua, fsg_store_nofua); static DEVICE_ATTR(file, 0644, fsg_show_file, fsg_store_file); +static DEVICE_ATTR(cdrom, 0644, fsg_show_cdrom, fsg_store_cdrom); #ifdef CONFIG_USB_MSC_PROFILING static DEVICE_ATTR(perf, 0644, fsg_show_perf, fsg_store_perf); #endif @@ -2911,6 +3181,9 @@ static struct fsg_common *fsg_common_init(struct fsg_common *common, rc = device_create_file(&curlun->dev, &dev_attr_nofua); if (rc) goto error_luns; + rc = device_create_file(&curlun->dev, &dev_attr_cdrom); + if (rc) + goto error_luns; #ifdef CONFIG_USB_MSC_PROFILING rc = device_create_file(&curlun->dev, &dev_attr_perf); if (rc) @@ -3051,6 +3324,7 @@ static void fsg_common_release(struct kref *ref) device_remove_file(&lun->dev, &dev_attr_nofua); device_remove_file(&lun->dev, &dev_attr_ro); device_remove_file(&lun->dev, &dev_attr_file); + device_remove_file(&lun->dev, &dev_attr_cdrom); fsg_lun_close(lun); device_unregister(&lun->dev); }