From 209aeff5b147e051a8841f64e64f5d0c04646349 Mon Sep 17 00:00:00 2001 From: Solomon Peachy Date: Sat, 2 Nov 2024 10:04:57 -0400 Subject: usb: Multiple improvements: * Support varying-sized logical sectors in a single build, instead of a hardcoded SECTOR_SIZE. * Properly truncate READ CAPACITY(10) if drive has over 2^32 sectors And fix potential alignment issues * Properly truncate READ_FORMAT_CAPACITY if drive has over 2^32 sectors * Support READ CAPACITY(16) Change-Id: I744b8629131f9ede31e1a972a6700b2244f2e798 --- firmware/usbstack/usb_storage.c | 217 +++++++++++++++++++++++++++------------- 1 file changed, 147 insertions(+), 70 deletions(-) diff --git a/firmware/usbstack/usb_storage.c b/firmware/usbstack/usb_storage.c index e226c15f99..617988b19f 100644 --- a/firmware/usbstack/usb_storage.c +++ b/firmware/usbstack/usb_storage.c @@ -106,8 +106,9 @@ #define SCSI_MODE_SENSE_10 0x5a #define SCSI_REQUEST_SENSE 0x03 #define SCSI_ALLOW_MEDIUM_REMOVAL 0x1e -#define SCSI_READ_CAPACITY 0x25 -#define SCSI_READ_FORMAT_CAPACITY 0x23 +#define SCSI_READ_CAPACITY_10 0x25 +#define SCSI_READ_CAPACITY_16 0x9e +#define USB_UFI_READ_FORMAT_CAPACITY 0x23 #define SCSI_READ_10 0x28 #define SCSI_WRITE_10 0x2a #define SCSI_START_STOP_UNIT 0x1b @@ -248,9 +249,16 @@ struct command_status_wrapper { unsigned char status; } __attribute__ ((packed)); -struct capacity { - unsigned int block_count; - unsigned int block_size; +struct capacity_10 { + unsigned char block_count[4]; + unsigned char block_size[4]; +} __attribute__ ((packed)); + +struct capacity_16 { + unsigned char block_count[8]; + unsigned char block_size[4]; + unsigned char flags[2]; + unsigned char lowest_aligned_lba[2]; } __attribute__ ((packed)); struct format_capacity { @@ -263,7 +271,8 @@ struct format_capacity { static union { unsigned char* transfer_buffer; struct inquiry_data* inquiry; - struct capacity* capacity_data; + struct capacity_10* capacity_data_10; + struct capacity_16* capacity_data_16; struct format_capacity* format_capacity_data; struct sense_data *sense_data; struct mode_sense_data_6 *ms_data_6; @@ -278,8 +287,8 @@ static char *cbw_buffer; static struct { sector_t sector; unsigned int count; - unsigned int orig_count; unsigned int cur_cmd; + unsigned int block_size; unsigned int tag; unsigned int lun; unsigned char *data[2]; @@ -501,6 +510,7 @@ void usb_storage_transfer_complete(int ep,int dir,int status,int length) #endif logf("transfer result for ep %d/%d %X %d", ep,dir,status, length); + switch(state) { case RECEIVING_BLOCKS: if(dir==USB_DIR_IN) { @@ -508,37 +518,37 @@ void usb_storage_transfer_complete(int ep,int dir,int status,int length) } logf("scsi write %llu %d", cur_cmd.sector, cur_cmd.count); if(status==0) { - if((unsigned int)length!=(SECTOR_SIZE* cur_cmd.count) + if((unsigned int)length!=(cur_cmd.block_size* cur_cmd.count) && (unsigned int)length!=WRITE_BUFFER_SIZE) { logf("unexpected length :%d",length); break; } sector_t next_sector = cur_cmd.sector + - (WRITE_BUFFER_SIZE/SECTOR_SIZE); + (WRITE_BUFFER_SIZE/cur_cmd.block_size); unsigned int next_count = cur_cmd.count - - MIN(cur_cmd.count,WRITE_BUFFER_SIZE/SECTOR_SIZE); + MIN(cur_cmd.count,WRITE_BUFFER_SIZE/cur_cmd.block_size); int next_select = !cur_cmd.data_select; if(next_count!=0) { /* Ask the host to send more, to the other buffer */ receive_block_data(cur_cmd.data[next_select], - MIN(WRITE_BUFFER_SIZE,next_count*SECTOR_SIZE)); + MIN(WRITE_BUFFER_SIZE,next_count*cur_cmd.block_size)); } /* Now write the data that just came in, while the host is sending the next bit */ #ifdef USB_USE_RAMDISK - memcpy(ramdisk_buffer + cur_cmd.sector*SECTOR_SIZE, + memcpy(ramdisk_buffer + cur_cmd.sector*cur_cmd.block_size, cur_cmd.data[cur_cmd.data_select], - MIN(WRITE_BUFFER_SIZE/SECTOR_SIZE, cur_cmd.count)*SECTOR_SIZE); + MIN(WRITE_BUFFER_SIZE/cur_cmd.block_size, cur_cmd.count)*cur_cmd.block_size); #else int result = USBSTOR_WRITE_SECTORS_FILTER(); if (result == 0) { result = storage_write_sectors(IF_MD(cur_cmd.lun,) cur_cmd.sector, - MIN(WRITE_BUFFER_SIZE/SECTOR_SIZE, cur_cmd.count), + MIN(WRITE_BUFFER_SIZE/cur_cmd.block_size, cur_cmd.count), cur_cmd.data[cur_cmd.data_select]); } @@ -712,25 +722,25 @@ static void send_and_read_next(void) cur_cmd.last_result = result; send_block_data(cur_cmd.data[cur_cmd.data_select], - MIN(READ_BUFFER_SIZE,cur_cmd.count*SECTOR_SIZE)); + MIN(READ_BUFFER_SIZE,cur_cmd.count*cur_cmd.block_size)); /* Switch buffers for the next one */ cur_cmd.data_select=!cur_cmd.data_select; - cur_cmd.sector+=(READ_BUFFER_SIZE/SECTOR_SIZE); - cur_cmd.count-=MIN(cur_cmd.count,READ_BUFFER_SIZE/SECTOR_SIZE); + cur_cmd.sector+=(READ_BUFFER_SIZE/cur_cmd.block_size); + cur_cmd.count-=MIN(cur_cmd.count,READ_BUFFER_SIZE/cur_cmd.block_size); if(cur_cmd.count!=0) { /* already read the next bit, so we can send it out immediately when the * current transfer completes. */ #ifdef USB_USE_RAMDISK memcpy(cur_cmd.data[cur_cmd.data_select], - ramdisk_buffer + cur_cmd.sector*SECTOR_SIZE, - MIN(READ_BUFFER_SIZE/SECTOR_SIZE, cur_cmd.count)*SECTOR_SIZE); + ramdisk_buffer + cur_cmd.sector*cur_cmd.block_size, + MIN(READ_BUFFER_SIZE/cur_cmd.block_size, cur_cmd.count)*cur_cmd.block_size); #else result = storage_read_sectors(IF_MD(cur_cmd.lun,) cur_cmd.sector, - MIN(READ_BUFFER_SIZE/SECTOR_SIZE, cur_cmd.count), + MIN(READ_BUFFER_SIZE/cur_cmd.block_size, cur_cmd.count), cur_cmd.data[cur_cmd.data_select]); if(cur_cmd.last_result == 0) cur_cmd.last_result = result; @@ -748,7 +758,6 @@ static void handle_scsi(struct command_block_wrapper* cbw) unsigned int block_size; bool lun_present=true; unsigned char lun = cbw->lun; - unsigned int block_size_mult = 1; if(letoh32(cbw->signature) != CBW_SIGNATURE) { logf("ums: bad cbw signature (%x)", cbw->signature); @@ -783,10 +792,14 @@ static void handle_scsi(struct command_block_wrapper* cbw) if(ejected[lun]) lun_present = false; + unsigned int block_size_mult = 1; /* Number of LOGICAL storage device blocks in each USB block */ #ifdef MAX_LOG_SECTOR_SIZE block_size_mult = disk_get_sector_multiplier(IF_MD(lun)); #endif + uint32_t bsize = block_size*block_size_mult; + sector_t bcount = block_count/block_size_mult; + cur_cmd.tag = cbw->tag; cur_cmd.lun = lun; cur_cmd.cur_cmd = cbw->command_block[0]; @@ -887,31 +900,33 @@ static void handle_scsi(struct command_block_wrapper* cbw) htobe16(sizeof(struct mode_sense_bdesc_longlba)); memset(tb.ms_data_10->block_descriptor.reserved,0,4); - memset(tb.ms_data_10->block_descriptor.num_blocks,0,8); #ifdef STORAGE_64BIT_SECTOR tb.ms_data_10->block_descriptor.num_blocks[2] = - ((block_count/block_size_mult) & 0xff00000000ULL)>>40; + (bcount & 0xff00000000ULL)>>40; tb.ms_data_10->block_descriptor.num_blocks[3] = - ((block_count/block_size_mult) & 0x00ff000000ULL)>>32; + (bcount & 0x00ff000000ULL)>>32; +#else + tb.ms_data_10->block_descriptor.num_blocks[2] = 0; + tb.ms_data_10->block_descriptor.num_blocks[3] = 0; #endif tb.ms_data_10->block_descriptor.num_blocks[4] = - ((block_count/block_size_mult) & 0xff000000)>>24; + (bcount & 0xff000000)>>24; tb.ms_data_10->block_descriptor.num_blocks[5] = - ((block_count/block_size_mult) & 0x00ff0000)>>16; + (bcount & 0x00ff0000)>>16; tb.ms_data_10->block_descriptor.num_blocks[6] = - ((block_count/block_size_mult) & 0x0000ff00)>>8; + (bcount & 0x0000ff00)>>8; tb.ms_data_10->block_descriptor.num_blocks[7] = - ((block_count/block_size_mult) & 0x000000ff); + (bcount & 0x000000ff); tb.ms_data_10->block_descriptor.block_size[0] = - ((block_size*block_size_mult) & 0xff000000)>>24; + (bsize & 0xff000000)>>24; tb.ms_data_10->block_descriptor.block_size[1] = - ((block_size*block_size_mult) & 0x00ff0000)>>16; + (bsize & 0x00ff0000)>>16; tb.ms_data_10->block_descriptor.block_size[2] = - ((block_size*block_size_mult) & 0x0000ff00)>>8; + (bsize & 0x0000ff00)>>8; tb.ms_data_10->block_descriptor.block_size[3] = - ((block_size*block_size_mult) & 0x000000ff); + (bsize & 0x000000ff); send_command_result(tb.ms_data_10, MIN(sizeof(struct mode_sense_data_10), length)); break; @@ -947,25 +962,24 @@ static void handle_scsi(struct command_block_wrapper* cbw) sizeof(struct mode_sense_bdesc_shortlba); tb.ms_data_6->block_descriptor.density_code = 0; tb.ms_data_6->block_descriptor.reserved = 0; - if(block_count/block_size_mult > 0xffffff) { + if(bcount > 0xffffff) { tb.ms_data_6->block_descriptor.num_blocks[0] = 0xff; tb.ms_data_6->block_descriptor.num_blocks[1] = 0xff; tb.ms_data_6->block_descriptor.num_blocks[2] = 0xff; - } - else { + } else { tb.ms_data_6->block_descriptor.num_blocks[0] = - ((block_count/block_size_mult) & 0xff0000)>>16; + (bcount & 0xff0000)>>16; tb.ms_data_6->block_descriptor.num_blocks[1] = - ((block_count/block_size_mult) & 0x00ff00)>>8; + (bcount & 0x00ff00)>>8; tb.ms_data_6->block_descriptor.num_blocks[2] = - ((block_count/block_size_mult) & 0x0000ff); + (bcount & 0x0000ff); } tb.ms_data_6->block_descriptor.block_size[0] = - ((block_size*block_size_mult) & 0xff0000)>>16; + (bsize & 0xff0000)>>16; tb.ms_data_6->block_descriptor.block_size[1] = - ((block_size*block_size_mult) & 0x00ff00)>>8; + (bsize & 0x00ff00)>>8; tb.ms_data_6->block_descriptor.block_size[2] = - ((block_size*block_size_mult) & 0x0000ff); + (bsize & 0x0000ff); send_command_result(tb.ms_data_6, MIN(sizeof(struct mode_sense_data_6), length)); break; @@ -1009,15 +1023,16 @@ static void handle_scsi(struct command_block_wrapper* cbw) send_csw(UMS_STATUS_GOOD); break; - case SCSI_READ_FORMAT_CAPACITY: { - logf("scsi read_format_capacity %d",lun); + case USB_UFI_READ_FORMAT_CAPACITY: { + logf("usb ufi read_format_capacity %d",lun); if(lun_present) { tb.format_capacity_data->following_length=htobe32(8); /* "block count" actually means "number of last block" */ - tb.format_capacity_data->block_count = - htobe32(block_count/block_size_mult - 1); - tb.format_capacity_data->block_size = - htobe32(block_size*block_size_mult); + if (bcount > 0xffffffff) + tb.format_capacity_data->block_count = 0xffffffff; + else + tb.format_capacity_data->block_count = htobe32(bcount - 1); + tb.format_capacity_data->block_size = htobe32(bsize); tb.format_capacity_data->block_size |= htobe32(SCSI_FORMAT_CAPACITY_FORMATTED_MEDIA); @@ -1033,20 +1048,30 @@ static void handle_scsi(struct command_block_wrapper* cbw) break; } - case SCSI_READ_CAPACITY: { - logf("scsi read_capacity %d",lun); + case SCSI_READ_CAPACITY_10: { + logf("scsi read_capacity10 %d",lun); if(lun_present) { /* "block count" actually means "number of last block" */ - tb.capacity_data->block_count = - htobe32(block_count/block_size_mult - 1); - tb.capacity_data->block_size = - htobe32(block_size*block_size_mult); - - send_command_result(tb.capacity_data, - MIN(sizeof(struct capacity), length)); - } - else { + if (bcount >= 0xffffffff) { + tb.capacity_data_10->block_count[0] = 0xff; + tb.capacity_data_10->block_count[1] = 0xff; + tb.capacity_data_10->block_count[2] = 0xff; + tb.capacity_data_10->block_count[3] = 0xff; + } else { + tb.capacity_data_10->block_count[0] = ((bcount-1) & 0xff000000)>>24; + tb.capacity_data_10->block_count[1] = ((bcount-1) & 0x00ff0000)>>16; + tb.capacity_data_10->block_count[2] = ((bcount-1) & 0x0000ff00)>>8; + tb.capacity_data_10->block_count[3] = ((bcount-1) & 0x000000ff); + } + tb.capacity_data_10->block_size[0] = (bsize & 0xff000000)>>24; + tb.capacity_data_10->block_size[1] = (bsize & 0x00ff0000)>>16; + tb.capacity_data_10->block_size[2] = (bsize & 0x0000ff00)>>8; + tb.capacity_data_10->block_size[3] = (bsize & 0x000000ff); + + send_command_result(tb.capacity_data_10, + MIN(sizeof(struct capacity_10), length)); + } else { send_command_failed_result(); cur_sense_data.sense_key=SENSE_NOT_READY; cur_sense_data.asc=ASC_MEDIUM_NOT_PRESENT; @@ -1054,7 +1079,59 @@ static void handle_scsi(struct command_block_wrapper* cbw) } break; } + case SCSI_READ_CAPACITY_16: { + logf("scsi read_capacity16 %d",lun); + if(lun_present) { + /* "block count" actually means "number of last block" */ +#ifdef STORAGE_64BIT_SECTOR + tb.capacity_data_16->block_count[2] = + ((bcount-1) & 0xff00000000ULL)>>40; + tb.capacity_data_16->block_count[3] = + ((bcount-1) & 0x00ff000000ULL)>>32; +#else + tb.capacity_data_16->block_count[2] = 0; + tb.capacity_data_16->block_count[3] = 0; +#endif + tb.capacity_data_16->block_count[4] = + ((bcount-1) & 0xff000000)>>24; + tb.capacity_data_16->block_count[5] = + ((bcount-1) & 0x00ff0000)>>16; + tb.capacity_data_16->block_count[6] = + ((bcount-1) & 0x0000ff00)>>8; + tb.capacity_data_16->block_count[7] = + ((bcount-1) & 0x000000ff); + + tb.ms_data_10->block_descriptor.block_size[0] = + (bsize & 0xff000000)>>24; + tb.ms_data_10->block_descriptor.block_size[1] = + (bsize & 0x00ff0000)>>16; + tb.ms_data_10->block_descriptor.block_size[2] = + (bsize & 0x0000ff00)>>8; + tb.ms_data_10->block_descriptor.block_size[3] = + (bsize & 0x000000ff); + + tb.capacity_data_16->block_size[0] = (bsize & 0xff000000)>>24; + tb.capacity_data_16->block_size[1] = (bsize & 0x00ff0000)>>16; + tb.capacity_data_16->block_size[2] = (bsize & 0x0000ff00)>>8; + tb.capacity_data_16->block_size[3] = (bsize & 0x000000ff); + + uint16_t flags = htobe16((1<<12) | find_first_set_bit(block_size_mult)); + memcpy(tb.capacity_data_16->flags, &flags, sizeof(flags)); + + tb.capacity_data_16->lowest_aligned_lba[0] = 0; + tb.capacity_data_16->lowest_aligned_lba[1] = 1; + + send_command_result(tb.capacity_data_16, + MIN(sizeof(struct capacity_16), length)); + } else { + send_command_failed_result(); + cur_sense_data.sense_key=SENSE_NOT_READY; + cur_sense_data.asc=ASC_MEDIUM_NOT_PRESENT; + cur_sense_data.ascq=0; + } + break; + } case SCSI_READ_10: logf("scsi read10 %d",lun); if(!lun_present) { @@ -1075,7 +1152,7 @@ static void handle_scsi(struct command_block_wrapper* cbw) cur_cmd.count = block_size_mult * (cbw->command_block[7] << 8 | cbw->command_block[8]); - cur_cmd.orig_count = cur_cmd.count; + cur_cmd.block_size = block_size; logf("scsi read %llu %d", cur_cmd.sector, cur_cmd.count); @@ -1088,12 +1165,12 @@ static void handle_scsi(struct command_block_wrapper* cbw) else { #ifdef USB_USE_RAMDISK memcpy(cur_cmd.data[cur_cmd.data_select], - ramdisk_buffer + cur_cmd.sector*SECTOR_SIZE, - MIN(READ_BUFFER_SIZE/SECTOR_SIZE,cur_cmd.count)*SECTOR_SIZE); + ramdisk_buffer + cur_cmd.sector*cur_cmd.block_size, + MIN(READ_BUFFER_SIZE/cur_cmd.block_size,cur_cmd.count)*cur_cmd.block_size); #else cur_cmd.last_result = storage_read_sectors(IF_MD(cur_cmd.lun,) cur_cmd.sector, - MIN(READ_BUFFER_SIZE/SECTOR_SIZE, cur_cmd.count), + MIN(READ_BUFFER_SIZE/cur_cmd.block_size, cur_cmd.count), cur_cmd.data[cur_cmd.data_select]); #endif send_and_read_next(); @@ -1126,7 +1203,7 @@ static void handle_scsi(struct command_block_wrapper* cbw) cbw->command_block[11] << 16 | cbw->command_block[12] << 8 | cbw->command_block[13]); - cur_cmd.orig_count = cur_cmd.count; + cur_cmd.block_size = block_size; logf("scsi read %llu %d", cur_cmd.sector, cur_cmd.count); @@ -1139,12 +1216,12 @@ static void handle_scsi(struct command_block_wrapper* cbw) else { #ifdef USB_USE_RAMDISK memcpy(cur_cmd.data[cur_cmd.data_select], - ramdisk_buffer + cur_cmd.sector*SECTOR_SIZE, - MIN(READ_BUFFER_SIZE/SECTOR_SIZE,cur_cmd.count)*SECTOR_SIZE); + ramdisk_buffer + cur_cmd.sector*cur_cmd.block_size, + MIN(READ_BUFFER_SIZE/cur_cmd.block_size,cur_cmd.count)*cur_cmd.block_size); #else cur_cmd.last_result = storage_read_sectors(IF_MD(cur_cmd.lun,) cur_cmd.sector, - MIN(READ_BUFFER_SIZE/SECTOR_SIZE, cur_cmd.count), + MIN(READ_BUFFER_SIZE/cur_cmd.block_size, cur_cmd.count), cur_cmd.data[cur_cmd.data_select]); #endif send_and_read_next(); @@ -1171,7 +1248,7 @@ static void handle_scsi(struct command_block_wrapper* cbw) cur_cmd.count = block_size_mult * (cbw->command_block[7] << 8 | cbw->command_block[8]); - cur_cmd.orig_count = cur_cmd.count; + cur_cmd.block_size = block_size; /* expect data */ if((cur_cmd.sector + cur_cmd.count) > block_count) { @@ -1182,7 +1259,7 @@ static void handle_scsi(struct command_block_wrapper* cbw) } else { receive_block_data(cur_cmd.data[0], - MIN(WRITE_BUFFER_SIZE, cur_cmd.count*SECTOR_SIZE)); + MIN(WRITE_BUFFER_SIZE, cur_cmd.count*cur_cmd.block_size)); } break; #ifdef STORAGE_64BIT_SECTOR @@ -1212,7 +1289,7 @@ static void handle_scsi(struct command_block_wrapper* cbw) cbw->command_block[11] << 16 | cbw->command_block[12] << 8 | cbw->command_block[13]); - cur_cmd.orig_count = cur_cmd.count; + cur_cmd.block_size = block_size; /* expect data */ if((cur_cmd.sector + cur_cmd.count) > block_count) { @@ -1223,7 +1300,7 @@ static void handle_scsi(struct command_block_wrapper* cbw) } else { receive_block_data(cur_cmd.data[0], - MIN(WRITE_BUFFER_SIZE, cur_cmd.count*SECTOR_SIZE)); + MIN(WRITE_BUFFER_SIZE, cur_cmd.count*cur_cmd.block_size)); } break; #endif -- cgit v1.2.3