diff options
Diffstat (limited to 'firmware/usbstack/usb_storage.c')
-rw-r--r-- | firmware/usbstack/usb_storage.c | 67 |
1 files changed, 55 insertions, 12 deletions
diff --git a/firmware/usbstack/usb_storage.c b/firmware/usbstack/usb_storage.c index 9d65f3b908..cee2f7700e 100644 --- a/firmware/usbstack/usb_storage.c +++ b/firmware/usbstack/usb_storage.c | |||
@@ -120,19 +120,35 @@ struct sense_data { | |||
120 | unsigned short SenseKeySpecific; | 120 | unsigned short SenseKeySpecific; |
121 | } __attribute__ ((packed)); | 121 | } __attribute__ ((packed)); |
122 | 122 | ||
123 | struct mode_sense_header_10 { | 123 | struct mode_sense_block_descriptor_longlba { |
124 | unsigned char number_of_blocks[8]; | ||
125 | unsigned char reserved[4]; | ||
126 | unsigned char block_size[4]; | ||
127 | } __attribute__ ((packed)); | ||
128 | |||
129 | struct mode_sense_block_descriptor_shortlba { | ||
130 | unsigned char density_code; | ||
131 | unsigned char number_of_blocks[3]; | ||
132 | unsigned char reserved; | ||
133 | unsigned char block_size[3]; | ||
134 | } __attribute__ ((packed)); | ||
135 | |||
136 | struct mode_sense_data_10 { | ||
124 | unsigned short mode_data_length; | 137 | unsigned short mode_data_length; |
125 | unsigned char medium_type; | 138 | unsigned char medium_type; |
126 | unsigned char device_specific; | 139 | unsigned char device_specific; |
127 | unsigned char reserved1[2]; | 140 | unsigned char longlba; |
141 | unsigned char reserved; | ||
128 | unsigned short block_descriptor_length; | 142 | unsigned short block_descriptor_length; |
143 | struct mode_sense_block_descriptor_longlba block_descriptor; | ||
129 | } __attribute__ ((packed)); | 144 | } __attribute__ ((packed)); |
130 | 145 | ||
131 | struct mode_sense_header_6 { | 146 | struct mode_sense_data_6 { |
132 | unsigned char mode_data_length; | 147 | unsigned char mode_data_length; |
133 | unsigned char medium_type; | 148 | unsigned char medium_type; |
134 | unsigned char device_specific; | 149 | unsigned char device_specific; |
135 | unsigned char block_descriptor_length; | 150 | unsigned char block_descriptor_length; |
151 | struct mode_sense_block_descriptor_shortlba block_descriptor; | ||
136 | } __attribute__ ((packed)); | 152 | } __attribute__ ((packed)); |
137 | 153 | ||
138 | struct command_block_wrapper { | 154 | struct command_block_wrapper { |
@@ -169,8 +185,8 @@ static struct inquiry_data* inquiry; | |||
169 | static struct capacity* capacity_data; | 185 | static struct capacity* capacity_data; |
170 | static struct format_capacity* format_capacity_data; | 186 | static struct format_capacity* format_capacity_data; |
171 | static struct sense_data *sense_data; | 187 | static struct sense_data *sense_data; |
172 | static struct mode_sense_header_6 *mode_sense_data_6; | 188 | static struct mode_sense_data_6 *mode_sense_data_6; |
173 | static struct mode_sense_header_10 *mode_sense_data_10; | 189 | static struct mode_sense_data_10 *mode_sense_data_10; |
174 | static struct report_lun_data *lun_data; | 190 | static struct report_lun_data *lun_data; |
175 | static struct command_status_wrapper* csw; | 191 | static struct command_status_wrapper* csw; |
176 | static char *max_lun; | 192 | static char *max_lun; |
@@ -529,12 +545,25 @@ static void handle_scsi(struct command_block_wrapper* cbw) | |||
529 | logf("scsi mode_sense_10 %d %X",lun,page_code); | 545 | logf("scsi mode_sense_10 %d %X",lun,page_code); |
530 | switch(page_code) { | 546 | switch(page_code) { |
531 | case 0x3f: | 547 | case 0x3f: |
532 | mode_sense_data_10->mode_data_length=0; | 548 | mode_sense_data_10->mode_data_length=sizeof(struct mode_sense_data_10); |
533 | mode_sense_data_10->medium_type=0; | 549 | mode_sense_data_10->medium_type=0; |
534 | mode_sense_data_10->device_specific=0; | 550 | mode_sense_data_10->device_specific=0; |
535 | mode_sense_data_10->block_descriptor_length=0; | 551 | mode_sense_data_10->reserved=0; |
552 | mode_sense_data_10->longlba=1; | ||
553 | mode_sense_data_10->block_descriptor_length=sizeof(struct mode_sense_block_descriptor_longlba); | ||
554 | memset(mode_sense_data_10->block_descriptor.reserved,0,4); | ||
555 | memset(mode_sense_data_10->block_descriptor.number_of_blocks,0,8); | ||
556 | mode_sense_data_10->block_descriptor.number_of_blocks[4]=((block_count/block_size_mult) & 0xff000000)>>24; | ||
557 | mode_sense_data_10->block_descriptor.number_of_blocks[5]=((block_count/block_size_mult) & 0x00ff0000)>>16; | ||
558 | mode_sense_data_10->block_descriptor.number_of_blocks[6]=((block_count/block_size_mult) & 0x0000ff00)>>8; | ||
559 | mode_sense_data_10->block_descriptor.number_of_blocks[7]=((block_count/block_size_mult) & 0x000000ff); | ||
560 | |||
561 | mode_sense_data_10->block_descriptor.block_size[0]=((block_size*block_size_mult) & 0xff000000)>>24; | ||
562 | mode_sense_data_10->block_descriptor.block_size[1]=((block_size*block_size_mult) & 0x00ff0000)>>16; | ||
563 | mode_sense_data_10->block_descriptor.block_size[2]=((block_size*block_size_mult) & 0x0000ff00)>>8; | ||
564 | mode_sense_data_10->block_descriptor.block_size[3]=((block_size*block_size_mult) & 0x000000ff); | ||
536 | send_command_result(mode_sense_data_10, | 565 | send_command_result(mode_sense_data_10, |
537 | MIN(sizeof(struct mode_sense_header_10), length)); | 566 | MIN(sizeof(struct mode_sense_data_10), length)); |
538 | break; | 567 | break; |
539 | default: | 568 | default: |
540 | usb_drv_stall(EP_MASS_STORAGE, true,true); | 569 | usb_drv_stall(EP_MASS_STORAGE, true,true); |
@@ -559,14 +588,28 @@ static void handle_scsi(struct command_block_wrapper* cbw) | |||
559 | switch(page_code) { | 588 | switch(page_code) { |
560 | case 0x3f: | 589 | case 0x3f: |
561 | /* All supported pages Since we support only one this is easy*/ | 590 | /* All supported pages Since we support only one this is easy*/ |
562 | mode_sense_data_6->mode_data_length=0; | 591 | mode_sense_data_6->mode_data_length=sizeof(struct mode_sense_data_6); |
563 | mode_sense_data_6->medium_type=0; | 592 | mode_sense_data_6->medium_type=0; |
564 | mode_sense_data_6->device_specific=0; | 593 | mode_sense_data_6->device_specific=0; |
565 | mode_sense_data_6->block_descriptor_length=0; | 594 | mode_sense_data_6->block_descriptor_length=sizeof(struct mode_sense_block_descriptor_shortlba); |
595 | mode_sense_data_6->block_descriptor.density_code=0; | ||
596 | mode_sense_data_6->block_descriptor.reserved=0; | ||
597 | if(block_count/block_size_mult > 0xffffff){ | ||
598 | mode_sense_data_6->block_descriptor.number_of_blocks[0]=0xff; | ||
599 | mode_sense_data_6->block_descriptor.number_of_blocks[1]=0xff; | ||
600 | mode_sense_data_6->block_descriptor.number_of_blocks[2]=0xff; | ||
601 | } | ||
602 | else { | ||
603 | mode_sense_data_6->block_descriptor.number_of_blocks[0]=((block_count/block_size_mult) & 0xff0000)>>16; | ||
604 | mode_sense_data_6->block_descriptor.number_of_blocks[1]=((block_count/block_size_mult) & 0x00ff00)>>8; | ||
605 | mode_sense_data_6->block_descriptor.number_of_blocks[2]=((block_count/block_size_mult) & 0x0000ff); | ||
606 | } | ||
607 | mode_sense_data_6->block_descriptor.block_size[0]=((block_size*block_size_mult) & 0xff0000)>>16; | ||
608 | mode_sense_data_6->block_descriptor.block_size[1]=((block_size*block_size_mult) & 0x00ff00)>>8; | ||
609 | mode_sense_data_6->block_descriptor.block_size[2]=((block_size*block_size_mult) & 0x0000ff); | ||
566 | send_command_result(mode_sense_data_6, | 610 | send_command_result(mode_sense_data_6, |
567 | MIN(sizeof(struct mode_sense_header_6), length)); | 611 | MIN(sizeof(struct mode_sense_data_6), length)); |
568 | break; | 612 | break; |
569 | /* TODO : windows does 1A, Power Condition. Do we need that ? */ | ||
570 | default: | 613 | default: |
571 | usb_drv_stall(EP_MASS_STORAGE, true,true); | 614 | usb_drv_stall(EP_MASS_STORAGE, true,true); |
572 | send_csw(UMS_STATUS_FAIL); | 615 | send_csw(UMS_STATUS_FAIL); |