| Index: apps/diskmode/SOURCES |
| — | — | @@ -1 +1,3 @@ |
| 2 | 2 | main.c
|
| | 3 | +usb.c
|
| | 4 | +ums.c
|
| Index: apps/diskmode/main.c |
| — | — | @@ -22,941 +22,43 @@ |
| 23 | 23 | //
|
| 24 | 24 |
|
| 25 | 25 |
|
| 26 | | -#define LITTLE_ENDIAN
|
| 27 | | -
|
| 28 | | -
|
| 29 | 26 | #include "emcoreapp.h"
|
| 30 | 27 | #include "libboot.h"
|
| | 28 | +#include "ums.h"
|
| | 29 | +#include "usb.h"
|
| 31 | 30 |
|
| 32 | 31 |
|
| 33 | | -#define SCSI_TEST_UNIT_READY 0x00
|
| 34 | | -#define SCSI_INQUIRY 0x12
|
| 35 | | -#define SCSI_MODE_SENSE_6 0x1a
|
| 36 | | -#define SCSI_MODE_SENSE_10 0x5a
|
| 37 | | -#define SCSI_REQUEST_SENSE 0x03
|
| 38 | | -#define SCSI_ALLOW_MEDIUM_REMOVAL 0x1e
|
| 39 | | -#define SCSI_READ_CAPACITY 0x25
|
| 40 | | -#define SCSI_READ_FORMAT_CAPACITY 0x23
|
| 41 | | -#define SCSI_READ_10 0x28
|
| 42 | | -#define SCSI_WRITE_10 0x2a
|
| 43 | | -#define SCSI_START_STOP_UNIT 0x1b
|
| 44 | | -#define SCSI_REPORT_LUNS 0xa0
|
| 45 | | -#define SCSI_WRITE_BUFFER 0x3b
|
| 46 | | -
|
| 47 | | -#define SENSE_NOT_READY 0x02
|
| 48 | | -#define SENSE_MEDIUM_ERROR 0x03
|
| 49 | | -#define SENSE_ILLEGAL_REQUEST 0x05
|
| 50 | | -#define SENSE_UNIT_ATTENTION 0x06
|
| 51 | | -
|
| 52 | | -#define ASC_MEDIUM_NOT_PRESENT 0x3a
|
| 53 | | -#define ASC_INVALID_FIELD_IN_CBD 0x24
|
| 54 | | -#define ASC_LBA_OUT_OF_RANGE 0x21
|
| 55 | | -#define ASC_WRITE_ERROR 0x0C
|
| 56 | | -#define ASC_READ_ERROR 0x11
|
| 57 | | -#define ASC_NOT_READY 0x04
|
| 58 | | -#define ASC_INVALID_COMMAND 0x20
|
| 59 | | -
|
| 60 | | -#define ASCQ_BECOMING_READY 0x01
|
| 61 | | -
|
| 62 | | -#define DIRECT_ACCESS_DEVICE 0x00
|
| 63 | | -#define DEVICE_REMOVABLE 0x80
|
| 64 | | -
|
| 65 | | -#define SCSI_FORMAT_CAPACITY_FORMATTED_MEDIA 0x02000000
|
| 66 | | -
|
| 67 | | -
|
| 68 | | -struct storage_info storage_info;
|
| 69 | | -
|
| 70 | | -enum storage_operation {
|
| 71 | | - OP_READ,
|
| 72 | | - OP_WRITE
|
| 73 | | -} pending_op;
|
| 74 | | -
|
| 75 | | -unsigned long pending_op_start;
|
| 76 | | -int pending_op_count;
|
| 77 | | -
|
| 78 | | -struct wakeup pending_op_wakeup;
|
| 79 | | -
|
| 80 | | -static const struct usb_devicedescriptor usb_devicedescriptor =
|
| 81 | | -{
|
| 82 | | - .bLength = sizeof(struct usb_devicedescriptor),
|
| 83 | | - .bDescriptorType = USB_DESCRIPTOR_TYPE_DEVICE,
|
| 84 | | - .bcdUSB = 0x0200,
|
| 85 | | - .bDeviceClass = 0,
|
| 86 | | - .bDeviceSubClass = 0,
|
| 87 | | - .bDeviceProtocol = 0,
|
| 88 | | - .bMaxPacketSize0 = 64,
|
| 89 | | - .idVendor = 0xffff,
|
| 90 | | - .idProduct = 0xe001,
|
| 91 | | - .bcdDevice = 2,
|
| 92 | | - .iManufacturer = 1,
|
| 93 | | - .iProduct = 2,
|
| 94 | | - .iSerialNumber = 0,
|
| 95 | | - .bNumConfigurations = 1,
|
| 96 | | -};
|
| 97 | | -
|
| 98 | | -static struct __attribute__((packed)) _usb_config1_descriptors
|
| 99 | | -{
|
| 100 | | - struct usb_configurationdescriptor c1;
|
| 101 | | - struct usb_interfacedescriptor c1_i0_a0;
|
| 102 | | - struct usb_endpointdescriptor c1_i0_a0_e1out;
|
| 103 | | - struct usb_endpointdescriptor c1_i0_a0_e1in;
|
| 104 | | -} usb_config1_descriptors =
|
| 105 | | -{
|
| 106 | | - .c1 =
|
| 107 | | - {
|
| 108 | | - .bLength = sizeof(struct usb_configurationdescriptor),
|
| 109 | | - .bDescriptorType = USB_DESCRIPTOR_TYPE_CONFIGURATION,
|
| 110 | | - .wTotalLength = sizeof(struct _usb_config1_descriptors),
|
| 111 | | - .bNumInterfaces = 1,
|
| 112 | | - .bConfigurationValue = 1,
|
| 113 | | - .iConfiguration = 0,
|
| 114 | | - .bmAttributes = { .buspowered = 1, .selfpowered = 1 },
|
| 115 | | - .bMaxPower = 100 / 2,
|
| 116 | | - },
|
| 117 | | - .c1_i0_a0 =
|
| 118 | | - {
|
| 119 | | - .bLength = sizeof(struct usb_interfacedescriptor),
|
| 120 | | - .bDescriptorType = USB_DESCRIPTOR_TYPE_INTERFACE,
|
| 121 | | - .bInterfaceNumber = 0,
|
| 122 | | - .bAlternateSetting = 0,
|
| 123 | | - .bNumEndpoints = 2,
|
| 124 | | - .bInterfaceClass = 0x08,
|
| 125 | | - .bInterfaceSubClass = 0x06,
|
| 126 | | - .bInterfaceProtocol = 0x50,
|
| 127 | | - .iInterface = 0,
|
| 128 | | - },
|
| 129 | | - .c1_i0_a0_e1out =
|
| 130 | | - {
|
| 131 | | - .bLength = sizeof(struct usb_endpointdescriptor),
|
| 132 | | - .bDescriptorType = USB_DESCRIPTOR_TYPE_ENDPOINT,
|
| 133 | | - .bEndpointAddress = { .number = 0, .direction = USB_ENDPOINT_DIRECTION_OUT },
|
| 134 | | - .bmAttributes = { .type = USB_ENDPOINT_ATTRIBUTE_TYPE_BULK },
|
| 135 | | - .wMaxPacketSize = 512,
|
| 136 | | - .bInterval = 1,
|
| 137 | | - },
|
| 138 | | - .c1_i0_a0_e1in =
|
| 139 | | - {
|
| 140 | | - .bLength = sizeof(struct usb_endpointdescriptor),
|
| 141 | | - .bDescriptorType = USB_DESCRIPTOR_TYPE_ENDPOINT,
|
| 142 | | - .bEndpointAddress = { .number = 0, .direction = USB_ENDPOINT_DIRECTION_IN },
|
| 143 | | - .bmAttributes = { .type = USB_ENDPOINT_ATTRIBUTE_TYPE_BULK },
|
| 144 | | - .wMaxPacketSize = 512,
|
| 145 | | - .bInterval = 1,
|
| 146 | | - },
|
| 147 | | -};
|
| 148 | | -
|
| 149 | | -static const struct usb_stringdescriptor usb_string_language =
|
| 150 | | -{
|
| 151 | | - .bLength = sizeof(usb_string_language) + sizeof(*usb_string_language.wString),
|
| 152 | | - .bDescriptorType = USB_DESCRIPTOR_TYPE_STRING,
|
| 153 | | - .wString = { 0x0409 },
|
| 154 | | -};
|
| 155 | | -
|
| 156 | | -static const struct usb_stringdescriptor usb_string_vendor =
|
| 157 | | -{
|
| 158 | | - .bLength = sizeof(usb_string_vendor) + sizeof(*usb_string_vendor.wString) * 14,
|
| 159 | | - .bDescriptorType = USB_DESCRIPTOR_TYPE_STRING,
|
| 160 | | - .wString = { 'f', 'r', 'e', 'e', 'm', 'y', 'i', 'p', 'o', 'd', '.', 'o', 'r', 'g' },
|
| 161 | | -};
|
| 162 | | -
|
| 163 | | -static const struct usb_stringdescriptor usb_string_product =
|
| 164 | | -{
|
| 165 | | - .bLength = sizeof(usb_string_product) + sizeof(*usb_string_product.wString) * 16,
|
| 166 | | - .bDescriptorType = USB_DESCRIPTOR_TYPE_STRING,
|
| 167 | | - .wString = { 'e', 'm', 'C', 'O', 'R', 'E', ' ', 'D', 'i', 's', 'k', ' ', 'm', 'o', 'd', 'e' },
|
| 168 | | -};
|
| 169 | | -
|
| 170 | | -static const struct usb_stringdescriptor* usb_stringdescriptors[] =
|
| 171 | | -{
|
| 172 | | - &usb_string_language,
|
| 173 | | - &usb_string_vendor,
|
| 174 | | - &usb_string_product,
|
| 175 | | -};
|
| 176 | | -
|
| 177 | | -struct __attribute__((packed,aligned(32))) command_block_wrapper
|
| 178 | | -{
|
| 179 | | - unsigned int signature;
|
| 180 | | - unsigned int tag;
|
| 181 | | - unsigned int data_transfer_length;
|
| 182 | | - unsigned char flags;
|
| 183 | | - unsigned char lun;
|
| 184 | | - unsigned char command_length;
|
| 185 | | - unsigned char command_block[16];
|
| 186 | | -};
|
| 187 | | -
|
| 188 | | -struct __attribute__((packed)) command_status_wrapper
|
| 189 | | -{
|
| 190 | | - unsigned int signature;
|
| 191 | | - unsigned int tag;
|
| 192 | | - unsigned int data_residue;
|
| 193 | | - unsigned char status;
|
| 194 | | -};
|
| 195 | | -
|
| 196 | | -struct __attribute__((packed)) inquiry_data
|
| 197 | | -{
|
| 198 | | - unsigned char DeviceType;
|
| 199 | | - unsigned char DeviceTypeModifier;
|
| 200 | | - unsigned char Versions;
|
| 201 | | - unsigned char Format;
|
| 202 | | - unsigned char AdditionalLength;
|
| 203 | | - unsigned char Reserved[2];
|
| 204 | | - unsigned char Capability;
|
| 205 | | - char VendorId[8];
|
| 206 | | - char ProductId[16];
|
| 207 | | - char ProductRevisionLevel[4];
|
| 208 | | -};
|
| 209 | | -
|
| 210 | | -struct __attribute__((packed)) report_lun_data
|
| 211 | | -{
|
| 212 | | - unsigned int lun_list_length;
|
| 213 | | - unsigned int reserved1;
|
| 214 | | - unsigned char luns[1][8];
|
| 215 | | -};
|
| 216 | | -
|
| 217 | | -struct __attribute__((packed)) sense_data
|
| 218 | | -{
|
| 219 | | - unsigned char ResponseCode;
|
| 220 | | - unsigned char Obsolete;
|
| 221 | | - unsigned char fei_sensekey;
|
| 222 | | - unsigned int Information;
|
| 223 | | - unsigned char AdditionalSenseLength;
|
| 224 | | - unsigned int CommandSpecificInformation;
|
| 225 | | - unsigned char AdditionalSenseCode;
|
| 226 | | - unsigned char AdditionalSenseCodeQualifier;
|
| 227 | | - unsigned char FieldReplaceableUnitCode;
|
| 228 | | - unsigned char SKSV;
|
| 229 | | - unsigned short SenseKeySpecific;
|
| 230 | | -};
|
| 231 | | -
|
| 232 | | -struct __attribute__((packed)) mode_sense_bdesc_longlba
|
| 233 | | -{
|
| 234 | | - unsigned char num_blocks[8];
|
| 235 | | - unsigned char reserved[4];
|
| 236 | | - unsigned char block_size[4];
|
| 237 | | -};
|
| 238 | | -
|
| 239 | | -struct __attribute__((packed)) mode_sense_bdesc_shortlba
|
| 240 | | -{
|
| 241 | | - unsigned char density_code;
|
| 242 | | - unsigned char num_blocks[3];
|
| 243 | | - unsigned char reserved;
|
| 244 | | - unsigned char block_size[3];
|
| 245 | | -};
|
| 246 | | -
|
| 247 | | -struct __attribute__((packed)) mode_sense_data_10
|
| 248 | | -{
|
| 249 | | - unsigned short mode_data_length;
|
| 250 | | - unsigned char medium_type;
|
| 251 | | - unsigned char device_specific;
|
| 252 | | - unsigned char longlba;
|
| 253 | | - unsigned char reserved;
|
| 254 | | - unsigned short block_descriptor_length;
|
| 255 | | - struct mode_sense_bdesc_longlba block_descriptor;
|
| 256 | | -};
|
| 257 | | -
|
| 258 | | -struct __attribute__((packed)) mode_sense_data_6
|
| 259 | | -{
|
| 260 | | - unsigned char mode_data_length;
|
| 261 | | - unsigned char medium_type;
|
| 262 | | - unsigned char device_specific;
|
| 263 | | - unsigned char block_descriptor_length;
|
| 264 | | - struct mode_sense_bdesc_shortlba block_descriptor;
|
| 265 | | -};
|
| 266 | | -
|
| 267 | | -struct __attribute__((packed)) capacity
|
| 268 | | -{
|
| 269 | | - unsigned int block_count;
|
| 270 | | - unsigned int block_size;
|
| 271 | | -};
|
| 272 | | -
|
| 273 | | -struct __attribute__((packed)) format_capacity
|
| 274 | | -{
|
| 275 | | - unsigned int following_length;
|
| 276 | | - unsigned int block_count;
|
| 277 | | - unsigned int block_size;
|
| 278 | | -};
|
| 279 | | -
|
| 280 | | -union __attribute__((aligned(32)))
|
| 281 | | -{
|
| 282 | | - struct inquiry_data inquiry;
|
| 283 | | - struct capacity capacity_data;
|
| 284 | | - struct format_capacity format_capacity_data;
|
| 285 | | - struct sense_data sense_data;
|
| 286 | | - struct mode_sense_data_6 ms_data_6;
|
| 287 | | - struct mode_sense_data_10 ms_data_10;
|
| 288 | | - struct report_lun_data lun_data;
|
| 289 | | - struct command_status_wrapper csw;
|
| 290 | | -} tb;
|
| 291 | | -
|
| 292 | | -static enum
|
| 293 | | -{
|
| 294 | | - WAITING_FOR_COMMAND,
|
| 295 | | - SENDING_BLOCKS,
|
| 296 | | - SENDING_RESULT,
|
| 297 | | - SENDING_FAILED_RESULT,
|
| 298 | | - RECEIVING_BLOCKS,
|
| 299 | | - WAITING_FOR_CSW_COMPLETION
|
| 300 | | -} state = WAITING_FOR_COMMAND;
|
| 301 | | -
|
| 302 | | -static struct
|
| 303 | | -{
|
| 304 | | - unsigned int sector;
|
| 305 | | - unsigned int count;
|
| 306 | | - unsigned int orig_count;
|
| 307 | | - unsigned int cur_cmd;
|
| 308 | | - unsigned int tag;
|
| 309 | | - unsigned int lun;
|
| 310 | | - unsigned int last_result;
|
| 311 | | -} cur_cmd;
|
| 312 | | -
|
| 313 | | -static struct
|
| 314 | | -{
|
| 315 | | - unsigned char sense_key;
|
| 316 | | - unsigned char information;
|
| 317 | | - unsigned char asc;
|
| 318 | | - unsigned char ascq;
|
| 319 | | -} cur_sense_data;
|
| 320 | | -
|
| 321 | | -static union usb_endpoint_number outep = { .number = 0, .direction = USB_ENDPOINT_DIRECTION_OUT };
|
| 322 | | -static union usb_endpoint_number inep = { .number = 0, .direction = USB_ENDPOINT_DIRECTION_IN };
|
| 323 | | -
|
| 324 | | -static struct command_block_wrapper cbw;
|
| 325 | | -static int maxpacket = 512;
|
| 326 | | -static int maxlen;
|
| 327 | | -static int length;
|
| 328 | | -static bool locked;
|
| 329 | | -static const struct usb_instance* usb;
|
| 330 | | -static volatile bool ejected = false;
|
| 331 | | -static uint8_t __attribute__((aligned(32))) storage_buffer[0x10000];
|
| 332 | | -
|
| 333 | | -struct bootinfo_t
|
| 334 | | -{
|
| 335 | | - bool valid;
|
| 336 | | - void* firmware;
|
| 337 | | - int size;
|
| 338 | | - void* app;
|
| 339 | | - int argc;
|
| 340 | | - const char** argv;
|
| 341 | | -};
|
| 342 | | -
|
| 343 | | -static void listen()
|
| 344 | | -{
|
| 345 | | - usb_start_rx(usb, outep, &cbw, sizeof(cbw));
|
| 346 | | -}
|
| 347 | | -
|
| 348 | | -static void send_csw(int status)
|
| 349 | | -{
|
| 350 | | - tb.csw.signature = 0x53425355;
|
| 351 | | - tb.csw.tag = cur_cmd.tag;
|
| 352 | | - tb.csw.data_residue = 0;
|
| 353 | | - tb.csw.status = status;
|
| 354 | | -
|
| 355 | | - state = WAITING_FOR_CSW_COMPLETION;
|
| 356 | | - usb_start_tx(usb, inep, &tb.csw, sizeof(tb.csw));
|
| 357 | | -
|
| 358 | | - if (!status)
|
| 359 | | - {
|
| 360 | | - cur_sense_data.sense_key = 0;
|
| 361 | | - cur_sense_data.information = 0;
|
| 362 | | - cur_sense_data.asc = 0;
|
| 363 | | - cur_sense_data.ascq = 0;
|
| 364 | | - }
|
| 365 | | -}
|
| 366 | | -
|
| 367 | | -static void receive_block_data(void* data, int size)
|
| 368 | | -{
|
| 369 | | - length = size;
|
| 370 | | - usb_start_rx(usb, outep, data, size);
|
| 371 | | - state = RECEIVING_BLOCKS;
|
| 372 | | -}
|
| 373 | | -
|
| 374 | | -static void send_block_data(void* data, int size)
|
| 375 | | -{
|
| 376 | | - length = size;
|
| 377 | | - usb_start_tx(usb, inep, data, size);
|
| 378 | | - state = SENDING_BLOCKS;
|
| 379 | | -}
|
| 380 | | -
|
| 381 | | -static void send_command_result(void* data, int size)
|
| 382 | | -{
|
| 383 | | - length = size;
|
| 384 | | - usb_start_tx(usb, inep, data, size);
|
| 385 | | - state = SENDING_RESULT;
|
| 386 | | -}
|
| 387 | | -
|
| 388 | | -static void send_command_failed_result()
|
| 389 | | -{
|
| 390 | | - usb_start_tx(usb, inep, NULL, 0);
|
| 391 | | - state = SENDING_FAILED_RESULT;
|
| 392 | | -}
|
| 393 | | -
|
| 394 | | -static void send_and_read_next()
|
| 395 | | -{
|
| 396 | | - if (cur_cmd.last_result)
|
| 397 | | - {
|
| 398 | | - send_csw(1);
|
| 399 | | - cur_sense_data.sense_key = SENSE_MEDIUM_ERROR;
|
| 400 | | - cur_sense_data.asc = ASC_READ_ERROR;
|
| 401 | | - cur_sense_data.ascq = 0;
|
| 402 | | - return;
|
| 403 | | - }
|
| 404 | | -
|
| 405 | | - pending_op_start = cur_cmd.sector;
|
| 406 | | - pending_op_count = cur_cmd.count;
|
| 407 | | - pending_op = OP_READ;
|
| 408 | | - wakeup_signal(&pending_op_wakeup);
|
| 409 | | -}
|
| 410 | | -
|
| 411 | | -static void copy_padded(char* dest, char* src, int len)
|
| 412 | | -{
|
| 413 | | - int i = 0;
|
| 414 | | - while (src[i] && i < len)
|
| 415 | | - {
|
| 416 | | - dest[i] = src[i];
|
| 417 | | - i++;
|
| 418 | | - }
|
| 419 | | - while(i < len) dest[i++] = ' ';
|
| 420 | | -}
|
| 421 | | -
|
| 422 | | -static void fill_inquiry()
|
| 423 | | -{
|
| 424 | | - memset(&tb.inquiry, 0, sizeof(tb.inquiry));
|
| 425 | | - copy_padded(tb.inquiry.VendorId, storage_info.vendor, sizeof(tb.inquiry.VendorId));
|
| 426 | | - copy_padded(tb.inquiry.ProductId, storage_info.product, sizeof(tb.inquiry.ProductId));
|
| 427 | | - copy_padded(tb.inquiry.ProductRevisionLevel, storage_info.revision, sizeof(tb.inquiry.ProductRevisionLevel));
|
| 428 | | -
|
| 429 | | - tb.inquiry.DeviceType = DIRECT_ACCESS_DEVICE;
|
| 430 | | - tb.inquiry.AdditionalLength = 0x1f;
|
| 431 | | - tb.inquiry.Versions = 4; // SPC-2
|
| 432 | | - tb.inquiry.Format = 2; // SPC-2/3 inquiry format
|
| 433 | | - tb.inquiry.DeviceTypeModifier = DEVICE_REMOVABLE;
|
| 434 | | -}
|
| 435 | | -
|
| 436 | | -static void handle_scsi(struct command_block_wrapper* cbw)
|
| 437 | | -{
|
| 438 | | - unsigned int length = cbw->data_transfer_length;
|
| 439 | | -
|
| 440 | | - if (cbw->signature != 0x43425355)
|
| 441 | | - {
|
| 442 | | - usb_set_stall(usb, outep, true);
|
| 443 | | - usb_set_stall(usb, inep, true);
|
| 444 | | - return;
|
| 445 | | - }
|
| 446 | | - cur_cmd.tag = cbw->tag;
|
| 447 | | - cur_cmd.lun = cbw->lun;
|
| 448 | | - cur_cmd.cur_cmd = cbw->command_block[0];
|
| 449 | | -
|
| 450 | | - switch (cbw->command_block[0])
|
| 451 | | - {
|
| 452 | | - case SCSI_TEST_UNIT_READY:
|
| 453 | | - if (!ejected) send_csw(0);
|
| 454 | | - else
|
| 455 | | - {
|
| 456 | | - send_csw(1);
|
| 457 | | - cur_sense_data.sense_key = SENSE_NOT_READY;
|
| 458 | | - cur_sense_data.asc = ASC_MEDIUM_NOT_PRESENT;
|
| 459 | | - cur_sense_data.ascq = 0;
|
| 460 | | - }
|
| 461 | | - break;
|
| 462 | | -
|
| 463 | | - case SCSI_REPORT_LUNS:
|
| 464 | | - {
|
| 465 | | - memset(&tb.lun_data, 0, sizeof(struct report_lun_data));
|
| 466 | | - tb.lun_data.lun_list_length = 0x08000000;
|
| 467 | | - send_command_result(&tb.lun_data, MIN(16, length));
|
| 468 | | - break;
|
| 469 | | - }
|
| 470 | | -
|
| 471 | | - case SCSI_INQUIRY:
|
| 472 | | - fill_inquiry();
|
| 473 | | - length = MIN(length, cbw->command_block[4]);
|
| 474 | | - send_command_result(&tb.inquiry, MIN(sizeof(tb.inquiry), length));
|
| 475 | | - break;
|
| 476 | | -
|
| 477 | | - case SCSI_REQUEST_SENSE:
|
| 478 | | - {
|
| 479 | | - tb.sense_data.ResponseCode = 0x70;
|
| 480 | | - tb.sense_data.Obsolete = 0;
|
| 481 | | - tb.sense_data.fei_sensekey = cur_sense_data.sense_key & 0x0f;
|
| 482 | | - tb.sense_data.Information = cur_sense_data.information;
|
| 483 | | - tb.sense_data.AdditionalSenseLength = 10;
|
| 484 | | - tb.sense_data.CommandSpecificInformation = 0;
|
| 485 | | - tb.sense_data.AdditionalSenseCode = cur_sense_data.asc;
|
| 486 | | - tb.sense_data.AdditionalSenseCodeQualifier = cur_sense_data.ascq;
|
| 487 | | - tb.sense_data.FieldReplaceableUnitCode = 0;
|
| 488 | | - tb.sense_data.SKSV = 0;
|
| 489 | | - tb.sense_data.SenseKeySpecific = 0;
|
| 490 | | - send_command_result(&tb.sense_data, MIN(sizeof(tb.sense_data), length));
|
| 491 | | - break;
|
| 492 | | - }
|
| 493 | | -
|
| 494 | | - case SCSI_MODE_SENSE_10:
|
| 495 | | - {
|
| 496 | | - if (ejected)
|
| 497 | | - {
|
| 498 | | - send_command_failed_result();
|
| 499 | | - cur_sense_data.sense_key = SENSE_NOT_READY;
|
| 500 | | - cur_sense_data.asc = ASC_MEDIUM_NOT_PRESENT;
|
| 501 | | - cur_sense_data.ascq = 0;
|
| 502 | | - break;
|
| 503 | | - }
|
| 504 | | - unsigned char page_code = cbw->command_block[2] & 0x3f;
|
| 505 | | - switch (page_code)
|
| 506 | | - {
|
| 507 | | - case 0x3f:
|
| 508 | | - tb.ms_data_10.mode_data_length = htobe16(sizeof(tb.ms_data_10) - 2);
|
| 509 | | - tb.ms_data_10.medium_type = 0;
|
| 510 | | - tb.ms_data_10.device_specific = 0;
|
| 511 | | - tb.ms_data_10.reserved = 0;
|
| 512 | | - tb.ms_data_10.longlba = 1;
|
| 513 | | - tb.ms_data_10.block_descriptor_length = htobe16(sizeof(tb.ms_data_10.block_descriptor));
|
| 514 | | -
|
| 515 | | - memset(tb.ms_data_10.block_descriptor.reserved, 0, 4);
|
| 516 | | - memset(tb.ms_data_10.block_descriptor.num_blocks, 0, 8);
|
| 517 | | -
|
| 518 | | - tb.ms_data_10.block_descriptor.num_blocks[4] = (storage_info.num_sectors & 0xff000000) >> 24;
|
| 519 | | - tb.ms_data_10.block_descriptor.num_blocks[5] = (storage_info.num_sectors & 0x00ff0000) >> 16;
|
| 520 | | - tb.ms_data_10.block_descriptor.num_blocks[6] = (storage_info.num_sectors & 0x0000ff00) >> 8;
|
| 521 | | - tb.ms_data_10.block_descriptor.num_blocks[7] = (storage_info.num_sectors & 0x000000ff);
|
| 522 | | -
|
| 523 | | - tb.ms_data_10.block_descriptor.block_size[0] = (storage_info.sector_size & 0xff000000) >> 24;
|
| 524 | | - tb.ms_data_10.block_descriptor.block_size[1] = (storage_info.sector_size & 0x00ff0000) >> 16;
|
| 525 | | - tb.ms_data_10.block_descriptor.block_size[2] = (storage_info.sector_size & 0x0000ff00) >> 8;
|
| 526 | | - tb.ms_data_10.block_descriptor.block_size[3] = (storage_info.sector_size & 0x000000ff);
|
| 527 | | - send_command_result(&tb.ms_data_10, MIN(sizeof(tb.ms_data_10), length));
|
| 528 | | - break;
|
| 529 | | - default:
|
| 530 | | - send_command_failed_result();
|
| 531 | | - cur_sense_data.sense_key = SENSE_ILLEGAL_REQUEST;
|
| 532 | | - cur_sense_data.asc = ASC_INVALID_FIELD_IN_CBD;
|
| 533 | | - cur_sense_data.ascq = 0;
|
| 534 | | - break;
|
| 535 | | - }
|
| 536 | | - break;
|
| 537 | | - }
|
| 538 | | -
|
| 539 | | - case SCSI_MODE_SENSE_6:
|
| 540 | | - {
|
| 541 | | - if (ejected)
|
| 542 | | - {
|
| 543 | | - send_command_failed_result();
|
| 544 | | - cur_sense_data.sense_key = SENSE_NOT_READY;
|
| 545 | | - cur_sense_data.asc = ASC_MEDIUM_NOT_PRESENT;
|
| 546 | | - cur_sense_data.ascq = 0;
|
| 547 | | - break;
|
| 548 | | - }
|
| 549 | | - unsigned char page_code = cbw->command_block[2] & 0x3f;
|
| 550 | | - switch (page_code)
|
| 551 | | - {
|
| 552 | | - case 0x3f:
|
| 553 | | - tb.ms_data_6.mode_data_length = sizeof(tb.ms_data_6) - 1;
|
| 554 | | - tb.ms_data_6.medium_type = 0;
|
| 555 | | - tb.ms_data_6.device_specific = 0;
|
| 556 | | - tb.ms_data_6.block_descriptor_length = sizeof(tb.ms_data_6.block_descriptor);
|
| 557 | | - tb.ms_data_6.block_descriptor.density_code = 0;
|
| 558 | | - tb.ms_data_6.block_descriptor.reserved = 0;
|
| 559 | | - if (storage_info.num_sectors > 0xffffff)
|
| 560 | | - {
|
| 561 | | - tb.ms_data_6.block_descriptor.num_blocks[0] = 0xff;
|
| 562 | | - tb.ms_data_6.block_descriptor.num_blocks[1] = 0xff;
|
| 563 | | - tb.ms_data_6.block_descriptor.num_blocks[2] = 0xff;
|
| 564 | | - }
|
| 565 | | - else
|
| 566 | | - {
|
| 567 | | - tb.ms_data_6.block_descriptor.num_blocks[0] = (storage_info.num_sectors & 0xff0000) >> 16;
|
| 568 | | - tb.ms_data_6.block_descriptor.num_blocks[1] = (storage_info.num_sectors & 0x00ff00) >> 8;
|
| 569 | | - tb.ms_data_6.block_descriptor.num_blocks[2] = (storage_info.num_sectors & 0x0000ff);
|
| 570 | | - }
|
| 571 | | - tb.ms_data_6.block_descriptor.block_size[0] = (storage_info.sector_size & 0xff0000) >> 16;
|
| 572 | | - tb.ms_data_6.block_descriptor.block_size[1] = (storage_info.sector_size & 0x00ff00) >> 8;
|
| 573 | | - tb.ms_data_6.block_descriptor.block_size[2] = (storage_info.sector_size & 0x0000ff);
|
| 574 | | - send_command_result(&tb.ms_data_6, MIN(sizeof(tb.ms_data_6), length));
|
| 575 | | - break;
|
| 576 | | - default:
|
| 577 | | - send_command_failed_result();
|
| 578 | | - cur_sense_data.sense_key = SENSE_ILLEGAL_REQUEST;
|
| 579 | | - cur_sense_data.asc = ASC_INVALID_FIELD_IN_CBD;
|
| 580 | | - cur_sense_data.ascq = 0;
|
| 581 | | - break;
|
| 582 | | - }
|
| 583 | | - break;
|
| 584 | | - }
|
| 585 | | -
|
| 586 | | - case SCSI_START_STOP_UNIT:
|
| 587 | | - if ((cbw->command_block[4] & 0xf3) == 2) ejected = true;
|
| 588 | | - send_csw(0);
|
| 589 | | - break;
|
| 590 | | -
|
| 591 | | - case SCSI_ALLOW_MEDIUM_REMOVAL:
|
| 592 | | - if ((cbw->command_block[4] & 0x03) == 0) locked = false;
|
| 593 | | - else locked = true;
|
| 594 | | - send_csw(0);
|
| 595 | | - break;
|
| 596 | | -
|
| 597 | | - case SCSI_READ_FORMAT_CAPACITY:
|
| 598 | | - {
|
| 599 | | - if (!ejected)
|
| 600 | | - {
|
| 601 | | - tb.format_capacity_data.following_length = 0x08000000;
|
| 602 | | - tb.format_capacity_data.block_count = htobe32(storage_info.num_sectors - 1);
|
| 603 | | - tb.format_capacity_data.block_size = htobe32(storage_info.sector_size);
|
| 604 | | - tb.format_capacity_data.block_size |= htobe32(SCSI_FORMAT_CAPACITY_FORMATTED_MEDIA);
|
| 605 | | - send_command_result(&tb.format_capacity_data, MIN(sizeof(tb.format_capacity_data), length));
|
| 606 | | - }
|
| 607 | | - else
|
| 608 | | - {
|
| 609 | | - send_command_failed_result();
|
| 610 | | - cur_sense_data.sense_key = SENSE_NOT_READY;
|
| 611 | | - cur_sense_data.asc = ASC_MEDIUM_NOT_PRESENT;
|
| 612 | | - cur_sense_data.ascq = 0;
|
| 613 | | - }
|
| 614 | | - break;
|
| 615 | | - }
|
| 616 | | -
|
| 617 | | - case SCSI_READ_CAPACITY:
|
| 618 | | - {
|
| 619 | | - if (!ejected)
|
| 620 | | - {
|
| 621 | | - tb.capacity_data.block_count = htobe32(storage_info.num_sectors - 1);
|
| 622 | | - tb.capacity_data.block_size = htobe32(storage_info.sector_size);
|
| 623 | | - send_command_result(&tb.capacity_data, MIN(sizeof(tb.capacity_data), length));
|
| 624 | | - }
|
| 625 | | - else
|
| 626 | | - {
|
| 627 | | - send_command_failed_result();
|
| 628 | | - cur_sense_data.sense_key = SENSE_NOT_READY;
|
| 629 | | - cur_sense_data.asc = ASC_MEDIUM_NOT_PRESENT;
|
| 630 | | - cur_sense_data.ascq = 0;
|
| 631 | | - }
|
| 632 | | - break;
|
| 633 | | - }
|
| 634 | | -
|
| 635 | | - case SCSI_READ_10:
|
| 636 | | - if (ejected)
|
| 637 | | - {
|
| 638 | | - send_command_failed_result();
|
| 639 | | - cur_sense_data.sense_key = SENSE_NOT_READY;
|
| 640 | | - cur_sense_data.asc = ASC_MEDIUM_NOT_PRESENT;
|
| 641 | | - cur_sense_data.ascq = 0;
|
| 642 | | - break;
|
| 643 | | - }
|
| 644 | | - cur_cmd.sector = (cbw->command_block[2] << 24 | cbw->command_block[3] << 16
|
| 645 | | - | cbw->command_block[4] << 8 | cbw->command_block[5]);
|
| 646 | | - cur_cmd.count = (cbw->command_block[7] << 8 | cbw->command_block[8]);
|
| 647 | | - cur_cmd.orig_count = cur_cmd.count;
|
| 648 | | -
|
| 649 | | - if ((cur_cmd.sector + cur_cmd.count) > storage_info.num_sectors)
|
| 650 | | - {
|
| 651 | | - send_csw(1);
|
| 652 | | - cur_sense_data.sense_key = SENSE_ILLEGAL_REQUEST;
|
| 653 | | - cur_sense_data.asc = ASC_LBA_OUT_OF_RANGE;
|
| 654 | | - cur_sense_data.ascq = 0;
|
| 655 | | - }
|
| 656 | | - else send_and_read_next();
|
| 657 | | - break;
|
| 658 | | -
|
| 659 | | - case SCSI_WRITE_10:
|
| 660 | | - if (ejected)
|
| 661 | | - {
|
| 662 | | - send_command_failed_result();
|
| 663 | | - cur_sense_data.sense_key = SENSE_NOT_READY;
|
| 664 | | - cur_sense_data.asc = ASC_MEDIUM_NOT_PRESENT;
|
| 665 | | - cur_sense_data.ascq = 0;
|
| 666 | | - break;
|
| 667 | | - }
|
| 668 | | - cur_cmd.sector = (cbw->command_block[2] << 24 | cbw->command_block[3] << 16
|
| 669 | | - | cbw->command_block[4] << 8 | cbw->command_block[5]);
|
| 670 | | - cur_cmd.count = (cbw->command_block[7] << 8 | cbw->command_block[8]);
|
| 671 | | - cur_cmd.orig_count = cur_cmd.count;
|
| 672 | | -
|
| 673 | | - if ((cur_cmd.sector + cur_cmd.count) > storage_info.num_sectors)
|
| 674 | | - {
|
| 675 | | - send_csw(1);
|
| 676 | | - cur_sense_data.sense_key = SENSE_ILLEGAL_REQUEST;
|
| 677 | | - cur_sense_data.asc = ASC_LBA_OUT_OF_RANGE;
|
| 678 | | - cur_sense_data.ascq = 0;
|
| 679 | | - }
|
| 680 | | - else {
|
| 681 | | - receive_block_data(storage_buffer, MIN(maxlen, cur_cmd.count * storage_info.sector_size));
|
| 682 | | - }
|
| 683 | | - break;
|
| 684 | | -
|
| 685 | | - case SCSI_WRITE_BUFFER:
|
| 686 | | - break;
|
| 687 | | -
|
| 688 | | - default:
|
| 689 | | - send_csw(1);
|
| 690 | | - cur_sense_data.sense_key = SENSE_ILLEGAL_REQUEST;
|
| 691 | | - cur_sense_data.asc = ASC_INVALID_COMMAND;
|
| 692 | | - cur_sense_data.ascq = 0;
|
| 693 | | - break;
|
| 694 | | - }
|
| 695 | | -}
|
| 696 | | -
|
| 697 | | -int handle_ctrl_request(const struct usb_instance* data, int interface, union usb_ep0_buffer* request, const void** response)
|
| 698 | | -{
|
| 699 | | - int size = -1;
|
| 700 | | - switch (request->setup.bmRequestType.type)
|
| 701 | | - {
|
| 702 | | - case USB_SETUP_BMREQUESTTYPE_TYPE_CLASS:
|
| 703 | | - switch (request->setup.bRequest.raw)
|
| 704 | | - {
|
| 705 | | - case 0xfe: // GET_MAX_LUN
|
| 706 | | - data->buffer->raw[0] = 0;
|
| 707 | | - size = 1;
|
| 708 | | - break;
|
| 709 | | - case 0xff: // STORAGE_RESET
|
| 710 | | - size = 0;
|
| 711 | | - break;
|
| 712 | | - default: break;
|
| 713 | | - }
|
| 714 | | - break;
|
| 715 | | - default: break;
|
| 716 | | - }
|
| 717 | | - return size;
|
| 718 | | -}
|
| 719 | | -
|
| 720 | | -void handle_set_altsetting(const struct usb_instance* data, int interface, int altsetting)
|
| 721 | | -{
|
| 722 | | - usb = data;
|
| 723 | | - state = WAITING_FOR_COMMAND;
|
| 724 | | - maxlen = maxpacket * MIN(usb_get_max_transfer_size(usb, outep), usb_get_max_transfer_size(usb, inep));
|
| 725 | | - usb_configure_ep(usb, outep, USB_ENDPOINT_TYPE_BULK, maxpacket);
|
| 726 | | - usb_configure_ep(usb, inep, USB_ENDPOINT_TYPE_BULK, maxpacket);
|
| 727 | | - listen();
|
| 728 | | -}
|
| 729 | | -
|
| 730 | | -void handle_unset_altsetting(const struct usb_instance* data, int interface, int altsetting)
|
| 731 | | -{
|
| 732 | | - usb_unconfigure_ep(usb, outep);
|
| 733 | | - usb_unconfigure_ep(usb, inep);
|
| 734 | | -}
|
| 735 | | -
|
| 736 | | -int handle_ep_ctrl_request(const struct usb_instance* data, int interface, int endpoint, union usb_ep0_buffer* request, const void** response)
|
| 737 | | -{
|
| 738 | | - int size = -1;
|
| 739 | | - switch (request->setup.bmRequestType.type)
|
| 740 | | - {
|
| 741 | | - case USB_SETUP_BMREQUESTTYPE_TYPE_STANDARD:
|
| 742 | | - switch (request->setup.bRequest.req)
|
| 743 | | - {
|
| 744 | | - case USB_SETUP_BREQUEST_CLEAR_FEATURE:
|
| 745 | | - if (request->setup.wLength || request->setup.wValue) break;
|
| 746 | | - listen();
|
| 747 | | - break;
|
| 748 | | - default: break;
|
| 749 | | - }
|
| 750 | | - break;
|
| 751 | | - default: break;
|
| 752 | | - }
|
| 753 | | - return size;
|
| 754 | | -}
|
| 755 | | -
|
| 756 | | -void handle_xfer_complete(const struct usb_instance* data, int ifnum, int epnum, int bytesleft)
|
| 757 | | -{
|
| 758 | | - length -= bytesleft;
|
| 759 | | - switch (state)
|
| 760 | | - {
|
| 761 | | - case RECEIVING_BLOCKS:
|
| 762 | | - if (length != storage_info.sector_size * cur_cmd.count && length != maxlen) break;
|
| 763 | | -
|
| 764 | | - pending_op_start = cur_cmd.sector;
|
| 765 | | - pending_op_count = cur_cmd.count;
|
| 766 | | - pending_op = OP_WRITE;
|
| 767 | | - wakeup_signal(&pending_op_wakeup);
|
| 768 | | - break;
|
| 769 | | - case WAITING_FOR_CSW_COMPLETION:
|
| 770 | | - state = WAITING_FOR_COMMAND;
|
| 771 | | - listen();
|
| 772 | | - break;
|
| 773 | | - case WAITING_FOR_COMMAND:
|
| 774 | | - handle_scsi(&cbw);
|
| 775 | | - break;
|
| 776 | | - case SENDING_RESULT:
|
| 777 | | - send_csw(0);
|
| 778 | | - break;
|
| 779 | | - case SENDING_FAILED_RESULT:
|
| 780 | | - send_csw(1);
|
| 781 | | - break;
|
| 782 | | - case SENDING_BLOCKS:
|
| 783 | | - if (!cur_cmd.count) send_csw(0);
|
| 784 | | - else send_and_read_next();
|
| 785 | | - break;
|
| 786 | | - }
|
| 787 | | -}
|
| 788 | | -
|
| 789 | | -void handle_timeout(const struct usb_instance* data, int interface, int endpoint, int bytesleft)
|
| 790 | | -{
|
| 791 | | - usb_set_stall(usb, outep, true);
|
| 792 | | - usb_set_stall(usb, inep, true);
|
| 793 | | -}
|
| 794 | | -
|
| 795 | | -static void handle_bus_reset(const struct usb_instance* data, int configuration, int interface, int highspeed)
|
| 796 | | -{
|
| 797 | | - maxpacket = highspeed ? 512 : 64;
|
| 798 | | - usb_config1_descriptors.c1_i0_a0_e1out.wMaxPacketSize = maxpacket;
|
| 799 | | - usb_config1_descriptors.c1_i0_a0_e1in.wMaxPacketSize = maxpacket;
|
| 800 | | -}
|
| 801 | | -
|
| 802 | | -static struct usb_endpoint usb_c1_i0_a0_ep1out =
|
| 803 | | -{
|
| 804 | | - .number = { .number = 0, .direction = USB_ENDPOINT_DIRECTION_OUT },
|
| 805 | | - .ctrl_request = handle_ep_ctrl_request,
|
| 806 | | - .xfer_complete = handle_xfer_complete,
|
| 807 | | -};
|
| 808 | | -
|
| 809 | | -static struct usb_endpoint usb_c1_i0_a0_ep1in =
|
| 810 | | -{
|
| 811 | | - .number = { .number = 0, .direction = USB_ENDPOINT_DIRECTION_IN },
|
| 812 | | - .ctrl_request = handle_ep_ctrl_request,
|
| 813 | | - .xfer_complete = handle_xfer_complete,
|
| 814 | | - .timeout = handle_timeout,
|
| 815 | | -};
|
| 816 | | -
|
| 817 | | -static const struct usb_altsetting usb_c1_i0_a0 =
|
| 818 | | -{
|
| 819 | | - .set_altsetting = handle_set_altsetting,
|
| 820 | | - .unset_altsetting = handle_unset_altsetting,
|
| 821 | | - .endpoint_count = 2,
|
| 822 | | - .endpoints =
|
| 823 | | - {
|
| 824 | | - &usb_c1_i0_a0_ep1out,
|
| 825 | | - &usb_c1_i0_a0_ep1in,
|
| 826 | | - },
|
| 827 | | -};
|
| 828 | | -
|
| 829 | | -static const struct usb_interface usb_c1_i0 =
|
| 830 | | -{
|
| 831 | | - .bus_reset = handle_bus_reset,
|
| 832 | | - .ctrl_request = handle_ctrl_request,
|
| 833 | | - .altsetting_count = 1,
|
| 834 | | - .altsettings =
|
| 835 | | - {
|
| 836 | | - &usb_c1_i0_a0,
|
| 837 | | - },
|
| 838 | | -};
|
| 839 | | -
|
| 840 | | -static const struct usb_configuration usb_c1 =
|
| 841 | | -{
|
| 842 | | - .descriptor = &usb_config1_descriptors.c1,
|
| 843 | | - .set_configuration = NULL,
|
| 844 | | - .unset_configuration = NULL,
|
| 845 | | - .interface_count = 1,
|
| 846 | | - .interfaces =
|
| 847 | | - {
|
| 848 | | - &usb_c1_i0,
|
| 849 | | - },
|
| 850 | | -};
|
| 851 | | -
|
| 852 | | -static const struct usb_configuration* usb_configurations[] =
|
| 853 | | -{
|
| 854 | | - &usb_c1,
|
| 855 | | -};
|
| 856 | | -
|
| 857 | | -struct emcorelib_header* loadlib(uint32_t identifier, uint32_t version, char* filename)
|
| 858 | | -{
|
| 859 | | - struct emcorelib_header* lib = get_library(identifier, version, LIBSOURCE_BOOTFLASH, filename);
|
| 860 | | - if (!lib) panicf(PANIC_KILLTHREAD, "Could not load %s", filename);
|
| 861 | | - return lib;
|
| 862 | | -}
|
| 863 | | -
|
| 864 | 32 | static void main(int argc, const char** argv)
|
| 865 | 33 | {
|
| 866 | | - cprintf(3, "Welcome to Disk mode! Please wait until your device is detected by your operating system. It should not take more than 1-2 minutes. In case of issues, please ask for support.\n\n");
|
| 867 | | - cprintf(3, "When you're done transferring files, please use your operating system's Eject/Unmount option before unplugging the device.\n\n");
|
| | 34 | + cputs(1, "Welcome to Disk Mode!\n\n"
|
| | 35 | + "This device will now behave\nlike an external hard drive.\n\n"
|
| | 36 | + "When you're finished, please\nproperly eject/unmount the disk.\n");
|
| | 37 | +
|
| | 38 | + ums_init();
|
| | 39 | + usb_prepare();
|
| 868 | 40 |
|
| 869 | | - storage_get_info(0, &storage_info);
|
| 870 | | -
|
| 871 | | - if (!storage_info.sector_size) panicf(PANIC_KILLTHREAD, "Sector size is zero!\n");
|
| 872 | | - if (!storage_info.num_sectors) panicf(PANIC_KILLTHREAD, "Number of sectors is zero!\n");
|
| 873 | | -
|
| 874 | 41 | disk_unmount(0);
|
| 875 | | -
|
| 876 | | - int i;
|
| 877 | | - uint32_t eps = usbmanager_get_available_endpoints();
|
| 878 | | - int ep_out = 0;
|
| 879 | | - int ep_in = 0;
|
| 880 | | - for (i = 1; i < 16; i++)
|
| 881 | | - if (eps & (1 << i))
|
| 882 | | - {
|
| 883 | | - ep_out = i;
|
| 884 | | - break;
|
| 885 | | - }
|
| 886 | | - for (i = 1; i < 16; i++)
|
| 887 | | - if (eps & (1 << (16 + i)))
|
| 888 | | - {
|
| 889 | | - ep_in = i;
|
| 890 | | - break;
|
| 891 | | - }
|
| 892 | | - if (!ep_out || !ep_in) panicf(PANIC_KILLTHREAD, "Not enough USB endpoints available!\n");
|
| 893 | | - usb_config1_descriptors.c1_i0_a0_e1out.bEndpointAddress.number = ep_out;
|
| 894 | | - usb_config1_descriptors.c1_i0_a0_e1in.bEndpointAddress.number = ep_in;
|
| 895 | | - usb_c1_i0_a0_ep1out.number.number = ep_out;
|
| 896 | | - usb_c1_i0_a0_ep1in.number.number = ep_in;
|
| 897 | | - outep.number = ep_out;
|
| 898 | | - inep.number = ep_in;
|
| 899 | | - int rc = usbmanager_install_custom(&usb_devicedescriptor, ARRAYLEN(usb_configurations), usb_configurations,
|
| 900 | | - ARRAYLEN(usb_stringdescriptors), usb_stringdescriptors, true);
|
| 901 | | - if (IS_ERR(rc)) {
|
| 902 | | - disk_mount(0);
|
| 903 | | - panicf(PANIC_KILLTHREAD, "Failed to register USB handler: %08X\n", rc);
|
| 904 | | - }
|
| | 42 | + usb_connect();
|
| 905 | 43 |
|
| 906 | | - wakeup_init(&pending_op_wakeup);
|
| | 44 | + cprintf(3, "Disk was ejected successfully.\nReturning to the bootmenu...\n\n");
|
| 907 | 45 |
|
| 908 | | - while (!ejected && usbmanager_get_connected()) {
|
| 909 | | - if (wakeup_wait(&pending_op_wakeup, 200000) == THREAD_OK) {
|
| 910 | | - if (pending_op == OP_READ) {
|
| 911 | | - //cprintf(3, "R(%u, %u)\n", pending_op_start, pending_op_count);
|
| 912 | | - storage_read_sectors_md(0, pending_op_start, pending_op_count, storage_buffer);
|
| 913 | | -
|
| 914 | | - send_block_data(storage_buffer, MIN(maxlen, cur_cmd.count * storage_info.sector_size));
|
| 915 | | - }
|
| 916 | | -
|
| 917 | | - if (pending_op == OP_WRITE) {
|
| 918 | | - //cprintf(3, "W(%u, %u)\n", pending_op_start, pending_op_count);
|
| 919 | | - storage_write_sectors_md(0, pending_op_start, pending_op_count, storage_buffer);
|
| 920 | | - }
|
| 921 | | -
|
| 922 | | - cur_cmd.sector += maxlen / storage_info.sector_size;
|
| 923 | | - cur_cmd.count -= MIN(cur_cmd.count, maxlen / storage_info.sector_size);
|
| 924 | | -
|
| 925 | | - if (pending_op == OP_WRITE) {
|
| 926 | | - if (cur_cmd.count) {
|
| 927 | | - receive_block_data(storage_buffer, MIN(maxlen, cur_cmd.count * storage_info.sector_size));
|
| 928 | | - }
|
| 929 | | - else send_csw(0);
|
| 930 | | - }
|
| 931 | | - }
|
| 932 | | - }
|
| 933 | | -
|
| 934 | | - usbmanager_uninstall_custom();
|
| 935 | | -
|
| 936 | | - cprintf(3, "Disk mode completed successfully. Returning to the bootmenu...\n\n");
|
| 937 | | -
|
| 938 | | - struct bootinfo_t bootinfo =
|
| 939 | | - {
|
| 940 | | - .valid = false,
|
| 941 | | - .firmware = NULL,
|
| 942 | | - .size = 0,
|
| 943 | | - .app = NULL,
|
| 944 | | - .argc = 0,
|
| 945 | | - .argv = NULL
|
| 946 | | - };
|
| 947 | | -
|
| 948 | | - struct emcorelib_header* libboot = loadlib(LIBBOOT_IDENTIFIER,
|
| 949 | | - LIBBOOT_API_VERSION, "libboot ");
|
| | 46 | + struct emcorelib_header* libboot = get_library(LIBBOOT_IDENTIFIER, LIBBOOT_API_VERSION,
|
| | 47 | + LIBSOURCE_BOOTFLASH, "libboot ");
|
| | 48 | + if (!libboot) panicf(PANIC_KILLTHREAD, "Could not load libboot");
|
| 950 | 49 | struct libboot_api* boot = (struct libboot_api*)libboot->api;
|
| 951 | 50 |
|
| 952 | | - boot->load_from_flash(&bootinfo.app, &bootinfo.size, false, "bootmenu", 0);
|
| 953 | | -
|
| 954 | | - if (!bootinfo.app) {
|
| | 51 | + int size = 0;
|
| | 52 | + void* app = NULL;
|
| | 53 | + boot->load_from_flash(&app, &size, false, "bootmenu", 0);
|
| | 54 | + if (!app)
|
| 955 | 55 | panicf(PANIC_KILLTHREAD, "Unable to start the bootmenu! Press MENU+SELECT to reboot your device.\n");
|
| 956 | | - }
|
| | 56 | +
|
| | 57 | + release_library(libboot);
|
| 957 | 58 |
|
| 958 | 59 | disk_mount(0);
|
| 959 | | - execimage(bootinfo.app, false, bootinfo.argc, bootinfo.argv);
|
| | 60 | + execimage(app, false, 0, NULL);
|
| 960 | 61 | }
|
| 961 | 62 |
|
| 962 | 63 |
|
| 963 | | -EMCORE_APP_HEADER("Disk mode", main, 127)
|
| | 64 | +EMCORE_APP_HEADER("Disk Mode", main, 127)
|
| | 65 | +
|
| Index: apps/diskmode/ums.c |
| — | — | @@ -0,0 +1,637 @@ |
| | 2 | +//
|
| | 3 | +//
|
| | 4 | +// Copyright 2013 TheSeven
|
| | 5 | +// Copyright 2014 user890104
|
| | 6 | +//
|
| | 7 | +//
|
| | 8 | +// This file is part of emCORE.
|
| | 9 | +//
|
| | 10 | +// emCORE is free software: you can redistribute it and/or
|
| | 11 | +// modify it under the terms of the GNU General Public License as
|
| | 12 | +// published by the Free Software Foundation, either version 2 of the
|
| | 13 | +// License, or (at your option) any later version.
|
| | 14 | +//
|
| | 15 | +// emCORE is distributed in the hope that it will be useful,
|
| | 16 | +// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
| | 17 | +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
| | 18 | +// See the GNU General Public License for more details.
|
| | 19 | +//
|
| | 20 | +// You should have received a copy of the GNU General Public License along
|
| | 21 | +// with emCORE. If not, see <http://www.gnu.org/licenses/>.
|
| | 22 | +//
|
| | 23 | +//
|
| | 24 | +
|
| | 25 | +
|
| | 26 | +#include "emcoreapp.h"
|
| | 27 | +#include "ums.h"
|
| | 28 | +#include "scsi.h"
|
| | 29 | +#include "usb.h"
|
| | 30 | +
|
| | 31 | +
|
| | 32 | +#define UMS_BUFSIZE 65536
|
| | 33 | +
|
| | 34 | +
|
| | 35 | +static struct
|
| | 36 | +{
|
| | 37 | + unsigned int sector;
|
| | 38 | + unsigned int count;
|
| | 39 | + unsigned int orig_count;
|
| | 40 | + unsigned int cur_cmd;
|
| | 41 | + unsigned int tag;
|
| | 42 | + unsigned int lun;
|
| | 43 | + unsigned int last_result;
|
| | 44 | +} cur_cmd;
|
| | 45 | +
|
| | 46 | +
|
| | 47 | +static struct
|
| | 48 | +{
|
| | 49 | + unsigned char sense_key;
|
| | 50 | + unsigned char information;
|
| | 51 | + unsigned char asc;
|
| | 52 | + unsigned char ascq;
|
| | 53 | +} cur_sense_data;
|
| | 54 | +
|
| | 55 | +
|
| | 56 | +struct __attribute__((packed,aligned(32))) command_block_wrapper
|
| | 57 | +{
|
| | 58 | + unsigned int signature;
|
| | 59 | + unsigned int tag;
|
| | 60 | + unsigned int data_transfer_length;
|
| | 61 | + unsigned char flags;
|
| | 62 | + unsigned char lun;
|
| | 63 | + unsigned char command_length;
|
| | 64 | + unsigned char command_block[16];
|
| | 65 | +};
|
| | 66 | +
|
| | 67 | +
|
| | 68 | +struct __attribute__((packed)) command_status_wrapper
|
| | 69 | +{
|
| | 70 | + unsigned int signature;
|
| | 71 | + unsigned int tag;
|
| | 72 | + unsigned int data_residue;
|
| | 73 | + unsigned char status;
|
| | 74 | +};
|
| | 75 | +
|
| | 76 | +
|
| | 77 | +union __attribute__((aligned(32)))
|
| | 78 | +{
|
| | 79 | + struct inquiry_data inquiry;
|
| | 80 | + struct capacity capacity_data;
|
| | 81 | + struct format_capacity format_capacity_data;
|
| | 82 | + struct sense_data sense_data;
|
| | 83 | + struct mode_sense_data_6 ms_data_6;
|
| | 84 | + struct mode_sense_data_10 ms_data_10;
|
| | 85 | + struct report_lun_data lun_data;
|
| | 86 | + struct command_status_wrapper csw;
|
| | 87 | +} tb;
|
| | 88 | +
|
| | 89 | +
|
| | 90 | +static enum
|
| | 91 | +{
|
| | 92 | + WAITING_FOR_COMMAND,
|
| | 93 | + SENDING_BLOCKS,
|
| | 94 | + SENDING_RESULT,
|
| | 95 | + SENDING_FAILED_RESULT,
|
| | 96 | + RECEIVING_BLOCKS,
|
| | 97 | + WAITING_FOR_CSW_COMPLETION
|
| | 98 | +} state = WAITING_FOR_COMMAND;
|
| | 99 | +
|
| | 100 | +static uint32_t length;
|
| | 101 | +static struct storage_info storage_info;
|
| | 102 | +static uint8_t __attribute__((aligned(32))) umsbuf[2][2][UMS_BUFSIZE];
|
| | 103 | +static union __attribute__((packed,aligned(32)))
|
| | 104 | +{
|
| | 105 | + struct command_block_wrapper cbw;
|
| | 106 | + uint8_t dummy;
|
| | 107 | +} cmdbuf;
|
| | 108 | +static uint8_t readbuf_current;
|
| | 109 | +static uint32_t readbuf_sector[2] = {0, 0};
|
| | 110 | +static int8_t readbuf_count[2] = {0, 0};
|
| | 111 | +static uint8_t read_blocked = false;
|
| | 112 | +static uint8_t writebuf_current;
|
| | 113 | +static uint32_t writebuf_sector;
|
| | 114 | +static uint8_t writebuf_count;
|
| | 115 | +static uint8_t writebuf_busy = false;
|
| | 116 | +static uint8_t writebuf_overrun = false;
|
| | 117 | +static uint32_t write_rc = 0;
|
| | 118 | +static bool locked = false;
|
| | 119 | +
|
| | 120 | +
|
| | 121 | +bool ums_ejected = false;
|
| | 122 | +
|
| | 123 | +
|
| | 124 | +static void listen()
|
| | 125 | +{
|
| | 126 | + usb_receive(&cmdbuf.cbw, sizeof(cmdbuf.cbw));
|
| | 127 | +}
|
| | 128 | +
|
| | 129 | +
|
| | 130 | +void ums_listen()
|
| | 131 | +{
|
| | 132 | + state = WAITING_FOR_COMMAND;
|
| | 133 | + listen();
|
| | 134 | +}
|
| | 135 | +
|
| | 136 | +
|
| | 137 | +static void send_csw(int status)
|
| | 138 | +{
|
| | 139 | + tb.csw.signature = 0x53425355;
|
| | 140 | + tb.csw.tag = cur_cmd.tag;
|
| | 141 | + tb.csw.data_residue = 0;
|
| | 142 | + tb.csw.status = status;
|
| | 143 | +
|
| | 144 | + state = WAITING_FOR_CSW_COMPLETION;
|
| | 145 | + usb_transmit(&tb.csw, sizeof(tb.csw));
|
| | 146 | +
|
| | 147 | + if (!status)
|
| | 148 | + {
|
| | 149 | + cur_sense_data.sense_key = 0;
|
| | 150 | + cur_sense_data.information = 0;
|
| | 151 | + cur_sense_data.asc = 0;
|
| | 152 | + cur_sense_data.ascq = 0;
|
| | 153 | + }
|
| | 154 | +}
|
| | 155 | +
|
| | 156 | +
|
| | 157 | +static void receive_block_data(void* data, int size)
|
| | 158 | +{
|
| | 159 | + length = size;
|
| | 160 | + usb_receive(data, size);
|
| | 161 | + state = RECEIVING_BLOCKS;
|
| | 162 | +}
|
| | 163 | +
|
| | 164 | +
|
| | 165 | +static void send_block_data(void* data, int size)
|
| | 166 | +{
|
| | 167 | + length = size;
|
| | 168 | + usb_transmit(data, size);
|
| | 169 | + state = SENDING_BLOCKS;
|
| | 170 | +}
|
| | 171 | +
|
| | 172 | +
|
| | 173 | +static void send_command_result(void* data, int size)
|
| | 174 | +{
|
| | 175 | + length = size;
|
| | 176 | + usb_transmit(data, size);
|
| | 177 | + state = SENDING_RESULT;
|
| | 178 | +}
|
| | 179 | +
|
| | 180 | +
|
| | 181 | +static void send_command_failed_result()
|
| | 182 | +{
|
| | 183 | + usb_transmit(NULL, 0);
|
| | 184 | + state = SENDING_FAILED_RESULT;
|
| | 185 | +}
|
| | 186 | +
|
| | 187 | +
|
| | 188 | +static void send_and_read_next()
|
| | 189 | +{
|
| | 190 | + int bufsize = UMS_BUFSIZE / storage_info.sector_size;
|
| | 191 | + int i;
|
| | 192 | + for (i = 0; i < 2; i++)
|
| | 193 | + {
|
| | 194 | + if (readbuf_sector[i] <= cur_cmd.sector && readbuf_sector[i] + bufsize > cur_cmd.sector)
|
| | 195 | + {
|
| | 196 | + if (readbuf_sector[i] + readbuf_count[i] <= cur_cmd.sector)
|
| | 197 | + {
|
| | 198 | + readbuf_current = i;
|
| | 199 | + read_blocked = true;
|
| | 200 | + enqueue_async();
|
| | 201 | + return;
|
| | 202 | + }
|
| | 203 | + int overlap = MIN(readbuf_sector[i] + readbuf_count[i] - cur_cmd.sector, cur_cmd.count);
|
| | 204 | + if (i == readbuf_current && readbuf_sector[i] + readbuf_count[i] < storage_info.num_sectors)
|
| | 205 | + {
|
| | 206 | + readbuf_current = !i;
|
| | 207 | + readbuf_sector[!i] = MAX(readbuf_sector[i] + readbuf_count[i], cur_cmd.sector + overlap);
|
| | 208 | + readbuf_count[!i] = 0;
|
| | 209 | + enqueue_async();
|
| | 210 | + }
|
| | 211 | + int offset = (cur_cmd.sector - readbuf_sector[i]) * storage_info.sector_size;
|
| | 212 | + send_block_data(umsbuf[0][i] + offset, overlap * storage_info.sector_size);
|
| | 213 | + cur_cmd.sector += overlap;
|
| | 214 | + cur_cmd.count -= overlap;
|
| | 215 | + return;
|
| | 216 | + }
|
| | 217 | + }
|
| | 218 | + readbuf_current ^= 1;
|
| | 219 | + readbuf_sector[readbuf_current] = cur_cmd.sector;
|
| | 220 | + readbuf_count[readbuf_current] = 0;
|
| | 221 | + read_blocked = true;
|
| | 222 | + enqueue_async();
|
| | 223 | +}
|
| | 224 | +
|
| | 225 | +
|
| | 226 | +static void copy_padded(char* dest, char* src, int len)
|
| | 227 | +{
|
| | 228 | + int i = 0;
|
| | 229 | + while (src[i] && i < len)
|
| | 230 | + {
|
| | 231 | + dest[i] = src[i];
|
| | 232 | + i++;
|
| | 233 | + }
|
| | 234 | + while(i < len) dest[i++] = ' ';
|
| | 235 | +}
|
| | 236 | +
|
| | 237 | +
|
| | 238 | +static void fill_inquiry()
|
| | 239 | +{
|
| | 240 | + memset(&tb.inquiry, 0, sizeof(tb.inquiry));
|
| | 241 | + copy_padded(tb.inquiry.VendorId, storage_info.vendor, sizeof(tb.inquiry.VendorId));
|
| | 242 | + copy_padded(tb.inquiry.ProductId, storage_info.product, sizeof(tb.inquiry.ProductId));
|
| | 243 | + copy_padded(tb.inquiry.ProductRevisionLevel, storage_info.revision, sizeof(tb.inquiry.ProductRevisionLevel));
|
| | 244 | +
|
| | 245 | + tb.inquiry.DeviceType = DIRECT_ACCESS_DEVICE;
|
| | 246 | + tb.inquiry.AdditionalLength = 0x1f;
|
| | 247 | + tb.inquiry.Versions = 4; // SPC-2
|
| | 248 | + tb.inquiry.Format = 2; // SPC-2/3 inquiry format
|
| | 249 | + tb.inquiry.DeviceTypeModifier = DEVICE_REMOVABLE;
|
| | 250 | +}
|
| | 251 | +
|
| | 252 | +
|
| | 253 | +static void handle_scsi(struct command_block_wrapper* cbw)
|
| | 254 | +{
|
| | 255 | + unsigned int length = cbw->data_transfer_length;
|
| | 256 | +
|
| | 257 | + if (cbw->signature != 0x43425355)
|
| | 258 | + {
|
| | 259 | + usb_stall();
|
| | 260 | + return;
|
| | 261 | + }
|
| | 262 | + cur_cmd.tag = cbw->tag;
|
| | 263 | + cur_cmd.lun = cbw->lun;
|
| | 264 | + cur_cmd.cur_cmd = cbw->command_block[0];
|
| | 265 | +
|
| | 266 | + switch (cbw->command_block[0])
|
| | 267 | + {
|
| | 268 | + case SCSI_TEST_UNIT_READY:
|
| | 269 | + if (!ums_ejected) send_csw(0);
|
| | 270 | + else
|
| | 271 | + {
|
| | 272 | + send_csw(1);
|
| | 273 | + cur_sense_data.sense_key = SENSE_NOT_READY;
|
| | 274 | + cur_sense_data.asc = ASC_MEDIUM_NOT_PRESENT;
|
| | 275 | + cur_sense_data.ascq = 0;
|
| | 276 | + }
|
| | 277 | + break;
|
| | 278 | +
|
| | 279 | + case SCSI_REPORT_LUNS:
|
| | 280 | + {
|
| | 281 | + memset(&tb.lun_data, 0, sizeof(struct report_lun_data));
|
| | 282 | + tb.lun_data.lun_list_length = 0x08000000;
|
| | 283 | + send_command_result(&tb.lun_data, MIN(16, length));
|
| | 284 | + break;
|
| | 285 | + }
|
| | 286 | +
|
| | 287 | + case SCSI_INQUIRY:
|
| | 288 | + fill_inquiry();
|
| | 289 | + length = MIN(length, cbw->command_block[4]);
|
| | 290 | + send_command_result(&tb.inquiry, MIN(sizeof(tb.inquiry), length));
|
| | 291 | + break;
|
| | 292 | +
|
| | 293 | + case SCSI_REQUEST_SENSE:
|
| | 294 | + {
|
| | 295 | + tb.sense_data.ResponseCode = 0x70;
|
| | 296 | + tb.sense_data.Obsolete = 0;
|
| | 297 | + tb.sense_data.fei_sensekey = cur_sense_data.sense_key & 0x0f;
|
| | 298 | + tb.sense_data.Information = cur_sense_data.information;
|
| | 299 | + tb.sense_data.AdditionalSenseLength = 10;
|
| | 300 | + tb.sense_data.CommandSpecificInformation = 0;
|
| | 301 | + tb.sense_data.AdditionalSenseCode = cur_sense_data.asc;
|
| | 302 | + tb.sense_data.AdditionalSenseCodeQualifier = cur_sense_data.ascq;
|
| | 303 | + tb.sense_data.FieldReplaceableUnitCode = 0;
|
| | 304 | + tb.sense_data.SKSV = 0;
|
| | 305 | + tb.sense_data.SenseKeySpecific = 0;
|
| | 306 | + send_command_result(&tb.sense_data, MIN(sizeof(tb.sense_data), length));
|
| | 307 | + break;
|
| | 308 | + }
|
| | 309 | +
|
| | 310 | + case SCSI_MODE_SENSE_10:
|
| | 311 | + {
|
| | 312 | + if (ums_ejected)
|
| | 313 | + {
|
| | 314 | + send_command_failed_result();
|
| | 315 | + cur_sense_data.sense_key = SENSE_NOT_READY;
|
| | 316 | + cur_sense_data.asc = ASC_MEDIUM_NOT_PRESENT;
|
| | 317 | + cur_sense_data.ascq = 0;
|
| | 318 | + break;
|
| | 319 | + }
|
| | 320 | + unsigned char page_code = cbw->command_block[2] & 0x3f;
|
| | 321 | + switch (page_code)
|
| | 322 | + {
|
| | 323 | + case 0x3f:
|
| | 324 | + tb.ms_data_10.mode_data_length = htobe16(sizeof(tb.ms_data_10) - 2);
|
| | 325 | + tb.ms_data_10.medium_type = 0;
|
| | 326 | + tb.ms_data_10.device_specific = 0;
|
| | 327 | + tb.ms_data_10.reserved = 0;
|
| | 328 | + tb.ms_data_10.longlba = 1;
|
| | 329 | + tb.ms_data_10.block_descriptor_length = htobe16(sizeof(tb.ms_data_10.block_descriptor));
|
| | 330 | +
|
| | 331 | + memset(tb.ms_data_10.block_descriptor.reserved, 0, 4);
|
| | 332 | + memset(tb.ms_data_10.block_descriptor.num_blocks, 0, 8);
|
| | 333 | +
|
| | 334 | + tb.ms_data_10.block_descriptor.num_blocks[4] = (storage_info.num_sectors & 0xff000000) >> 24;
|
| | 335 | + tb.ms_data_10.block_descriptor.num_blocks[5] = (storage_info.num_sectors & 0x00ff0000) >> 16;
|
| | 336 | + tb.ms_data_10.block_descriptor.num_blocks[6] = (storage_info.num_sectors & 0x0000ff00) >> 8;
|
| | 337 | + tb.ms_data_10.block_descriptor.num_blocks[7] = (storage_info.num_sectors & 0x000000ff);
|
| | 338 | +
|
| | 339 | + tb.ms_data_10.block_descriptor.block_size[0] = (storage_info.sector_size & 0xff000000) >> 24;
|
| | 340 | + tb.ms_data_10.block_descriptor.block_size[1] = (storage_info.sector_size & 0x00ff0000) >> 16;
|
| | 341 | + tb.ms_data_10.block_descriptor.block_size[2] = (storage_info.sector_size & 0x0000ff00) >> 8;
|
| | 342 | + tb.ms_data_10.block_descriptor.block_size[3] = (storage_info.sector_size & 0x000000ff);
|
| | 343 | + send_command_result(&tb.ms_data_10, MIN(sizeof(tb.ms_data_10), length));
|
| | 344 | + break;
|
| | 345 | + default:
|
| | 346 | + send_command_failed_result();
|
| | 347 | + cur_sense_data.sense_key = SENSE_ILLEGAL_REQUEST;
|
| | 348 | + cur_sense_data.asc = ASC_INVALID_FIELD_IN_CBD;
|
| | 349 | + cur_sense_data.ascq = 0;
|
| | 350 | + break;
|
| | 351 | + }
|
| | 352 | + break;
|
| | 353 | + }
|
| | 354 | +
|
| | 355 | + case SCSI_MODE_SENSE_6:
|
| | 356 | + {
|
| | 357 | + if (ums_ejected)
|
| | 358 | + {
|
| | 359 | + send_command_failed_result();
|
| | 360 | + cur_sense_data.sense_key = SENSE_NOT_READY;
|
| | 361 | + cur_sense_data.asc = ASC_MEDIUM_NOT_PRESENT;
|
| | 362 | + cur_sense_data.ascq = 0;
|
| | 363 | + break;
|
| | 364 | + }
|
| | 365 | + unsigned char page_code = cbw->command_block[2] & 0x3f;
|
| | 366 | + switch (page_code)
|
| | 367 | + {
|
| | 368 | + case 0x3f:
|
| | 369 | + tb.ms_data_6.mode_data_length = sizeof(tb.ms_data_6) - 1;
|
| | 370 | + tb.ms_data_6.medium_type = 0;
|
| | 371 | + tb.ms_data_6.device_specific = 0;
|
| | 372 | + tb.ms_data_6.block_descriptor_length = sizeof(tb.ms_data_6.block_descriptor);
|
| | 373 | + tb.ms_data_6.block_descriptor.density_code = 0;
|
| | 374 | + tb.ms_data_6.block_descriptor.reserved = 0;
|
| | 375 | + if (storage_info.num_sectors > 0xffffff)
|
| | 376 | + {
|
| | 377 | + tb.ms_data_6.block_descriptor.num_blocks[0] = 0xff;
|
| | 378 | + tb.ms_data_6.block_descriptor.num_blocks[1] = 0xff;
|
| | 379 | + tb.ms_data_6.block_descriptor.num_blocks[2] = 0xff;
|
| | 380 | + }
|
| | 381 | + else
|
| | 382 | + {
|
| | 383 | + tb.ms_data_6.block_descriptor.num_blocks[0] = (storage_info.num_sectors & 0xff0000) >> 16;
|
| | 384 | + tb.ms_data_6.block_descriptor.num_blocks[1] = (storage_info.num_sectors & 0x00ff00) >> 8;
|
| | 385 | + tb.ms_data_6.block_descriptor.num_blocks[2] = (storage_info.num_sectors & 0x0000ff);
|
| | 386 | + }
|
| | 387 | + tb.ms_data_6.block_descriptor.block_size[0] = (storage_info.sector_size & 0xff0000) >> 16;
|
| | 388 | + tb.ms_data_6.block_descriptor.block_size[1] = (storage_info.sector_size & 0x00ff00) >> 8;
|
| | 389 | + tb.ms_data_6.block_descriptor.block_size[2] = (storage_info.sector_size & 0x0000ff);
|
| | 390 | + send_command_result(&tb.ms_data_6, MIN(sizeof(tb.ms_data_6), length));
|
| | 391 | + break;
|
| | 392 | + default:
|
| | 393 | + send_command_failed_result();
|
| | 394 | + cur_sense_data.sense_key = SENSE_ILLEGAL_REQUEST;
|
| | 395 | + cur_sense_data.asc = ASC_INVALID_FIELD_IN_CBD;
|
| | 396 | + cur_sense_data.ascq = 0;
|
| | 397 | + break;
|
| | 398 | + }
|
| | 399 | + break;
|
| | 400 | + }
|
| | 401 | +
|
| | 402 | + case SCSI_START_STOP_UNIT:
|
| | 403 | + if ((cbw->command_block[4] & 0xf3) == 2) ums_ejected = true;
|
| | 404 | + send_csw(0);
|
| | 405 | + break;
|
| | 406 | +
|
| | 407 | + case SCSI_ALLOW_MEDIUM_REMOVAL:
|
| | 408 | + if ((cbw->command_block[4] & 0x03) == 0) locked = false;
|
| | 409 | + else locked = true;
|
| | 410 | + send_csw(0);
|
| | 411 | + break;
|
| | 412 | +
|
| | 413 | + case SCSI_READ_FORMAT_CAPACITY:
|
| | 414 | + {
|
| | 415 | + if (!ums_ejected)
|
| | 416 | + {
|
| | 417 | + tb.format_capacity_data.following_length = 0x08000000;
|
| | 418 | + tb.format_capacity_data.block_count = swap32(storage_info.num_sectors - 1);
|
| | 419 | + tb.format_capacity_data.block_size = swap32(storage_info.sector_size);
|
| | 420 | + tb.format_capacity_data.block_size |= swap32(SCSI_FORMAT_CAPACITY_FORMATTED_MEDIA);
|
| | 421 | + send_command_result(&tb.format_capacity_data, MIN(sizeof(tb.format_capacity_data), length));
|
| | 422 | + }
|
| | 423 | + else
|
| | 424 | + {
|
| | 425 | + send_command_failed_result();
|
| | 426 | + cur_sense_data.sense_key = SENSE_NOT_READY;
|
| | 427 | + cur_sense_data.asc = ASC_MEDIUM_NOT_PRESENT;
|
| | 428 | + cur_sense_data.ascq = 0;
|
| | 429 | + }
|
| | 430 | + break;
|
| | 431 | + }
|
| | 432 | +
|
| | 433 | + case SCSI_READ_CAPACITY:
|
| | 434 | + {
|
| | 435 | + if (!ums_ejected)
|
| | 436 | + {
|
| | 437 | + tb.capacity_data.block_count = swap32(storage_info.num_sectors - 1);
|
| | 438 | + tb.capacity_data.block_size = swap32(storage_info.sector_size);
|
| | 439 | + send_command_result(&tb.capacity_data, MIN(sizeof(tb.capacity_data), length));
|
| | 440 | + }
|
| | 441 | + else
|
| | 442 | + {
|
| | 443 | + send_command_failed_result();
|
| | 444 | + cur_sense_data.sense_key = SENSE_NOT_READY;
|
| | 445 | + cur_sense_data.asc = ASC_MEDIUM_NOT_PRESENT;
|
| | 446 | + cur_sense_data.ascq = 0;
|
| | 447 | + }
|
| | 448 | + break;
|
| | 449 | + }
|
| | 450 | +
|
| | 451 | + case SCSI_READ_10:
|
| | 452 | + if (ums_ejected)
|
| | 453 | + {
|
| | 454 | + send_command_failed_result();
|
| | 455 | + cur_sense_data.sense_key = SENSE_NOT_READY;
|
| | 456 | + cur_sense_data.asc = ASC_MEDIUM_NOT_PRESENT;
|
| | 457 | + cur_sense_data.ascq = 0;
|
| | 458 | + break;
|
| | 459 | + }
|
| | 460 | + cur_cmd.sector = (cbw->command_block[2] << 24 | cbw->command_block[3] << 16
|
| | 461 | + | cbw->command_block[4] << 8 | cbw->command_block[5]);
|
| | 462 | + cur_cmd.count = (cbw->command_block[7] << 8 | cbw->command_block[8]);
|
| | 463 | + cur_cmd.orig_count = cur_cmd.count;
|
| | 464 | +
|
| | 465 | + if ((cur_cmd.sector + cur_cmd.count) > storage_info.num_sectors)
|
| | 466 | + {
|
| | 467 | + send_csw(1);
|
| | 468 | + cur_sense_data.sense_key = SENSE_ILLEGAL_REQUEST;
|
| | 469 | + cur_sense_data.asc = ASC_LBA_OUT_OF_RANGE;
|
| | 470 | + cur_sense_data.ascq = 0;
|
| | 471 | + }
|
| | 472 | + else if (!cur_cmd.count) send_csw(0);
|
| | 473 | + else send_and_read_next();
|
| | 474 | + break;
|
| | 475 | +
|
| | 476 | + case SCSI_WRITE_10:
|
| | 477 | + if (ums_ejected)
|
| | 478 | + {
|
| | 479 | + send_command_failed_result();
|
| | 480 | + cur_sense_data.sense_key = SENSE_NOT_READY;
|
| | 481 | + cur_sense_data.asc = ASC_MEDIUM_NOT_PRESENT;
|
| | 482 | + cur_sense_data.ascq = 0;
|
| | 483 | + break;
|
| | 484 | + }
|
| | 485 | + cur_cmd.sector = (cbw->command_block[2] << 24 | cbw->command_block[3] << 16
|
| | 486 | + | cbw->command_block[4] << 8 | cbw->command_block[5]);
|
| | 487 | + cur_cmd.count = (cbw->command_block[7] << 8 | cbw->command_block[8]);
|
| | 488 | + cur_cmd.orig_count = cur_cmd.count;
|
| | 489 | +
|
| | 490 | + if ((cur_cmd.sector + cur_cmd.count) > storage_info.num_sectors)
|
| | 491 | + {
|
| | 492 | + send_csw(1);
|
| | 493 | + cur_sense_data.sense_key = SENSE_ILLEGAL_REQUEST;
|
| | 494 | + cur_sense_data.asc = ASC_LBA_OUT_OF_RANGE;
|
| | 495 | + cur_sense_data.ascq = 0;
|
| | 496 | + }
|
| | 497 | + else if (!cur_cmd.count) send_csw(0);
|
| | 498 | + else
|
| | 499 | + receive_block_data(umsbuf[1][!writebuf_current],
|
| | 500 | + MIN(UMS_BUFSIZE, cur_cmd.count * storage_info.sector_size));
|
| | 501 | + break;
|
| | 502 | +
|
| | 503 | + case SCSI_WRITE_BUFFER:
|
| | 504 | + break;
|
| | 505 | +
|
| | 506 | + default:
|
| | 507 | + send_csw(1);
|
| | 508 | + cur_sense_data.sense_key = SENSE_ILLEGAL_REQUEST;
|
| | 509 | + cur_sense_data.asc = ASC_INVALID_COMMAND;
|
| | 510 | + cur_sense_data.ascq = 0;
|
| | 511 | + break;
|
| | 512 | + }
|
| | 513 | +}
|
| | 514 | +
|
| | 515 | +
|
| | 516 | +void ums_init()
|
| | 517 | +{
|
| | 518 | + storage_get_info(0, &storage_info);
|
| | 519 | + if (!storage_info.sector_size) panicf(PANIC_KILLTHREAD, "Sector size is zero!\n");
|
| | 520 | + if (!storage_info.num_sectors) panicf(PANIC_KILLTHREAD, "Number of sectors is zero!\n");
|
| | 521 | +}
|
| | 522 | +
|
| | 523 | +
|
| | 524 | +void update_readcache()
|
| | 525 | +{
|
| | 526 | + int count = length / storage_info.sector_size;
|
| | 527 | + int i;
|
| | 528 | + for (i = 0; i < 2; i++)
|
| | 529 | + if ((readbuf_sector[i] <= cur_cmd.sector && readbuf_sector[i] + readbuf_count[i] > cur_cmd.sector)
|
| | 530 | + || (readbuf_sector[i] <= cur_cmd.sector + count
|
| | 531 | + && readbuf_sector[i] + readbuf_count[i] > cur_cmd.sector + count))
|
| | 532 | + {
|
| | 533 | + int roffset = MAX(0, (int)(cur_cmd.sector - readbuf_sector[i]));
|
| | 534 | + int woffset = MAX(0, (int)(readbuf_sector[i] - cur_cmd.sector));
|
| | 535 | + int len = MIN(readbuf_count[i] - roffset, count - woffset);
|
| | 536 | + roffset *= storage_info.sector_size;
|
| | 537 | + woffset *= storage_info.sector_size;
|
| | 538 | + len *= storage_info.sector_size;
|
| | 539 | + memcpy(umsbuf[0][i] + roffset, umsbuf[1][writebuf_current] + woffset, len);
|
| | 540 | + }
|
| | 541 | +}
|
| | 542 | +
|
| | 543 | +
|
| | 544 | +static void writebuf_push()
|
| | 545 | +{
|
| | 546 | + int count = length / storage_info.sector_size;
|
| | 547 | + writebuf_current ^= 1;
|
| | 548 | + writebuf_sector = cur_cmd.sector;
|
| | 549 | + cur_cmd.sector += count;
|
| | 550 | + writebuf_count = count;
|
| | 551 | + cur_cmd.count -= count;
|
| | 552 | + writebuf_busy = true;
|
| | 553 | + int remaining = cur_cmd.count * storage_info.sector_size;
|
| | 554 | + if (!cur_cmd.count)
|
| | 555 | + {
|
| | 556 | + if (IS_ERR(write_rc))
|
| | 557 | + {
|
| | 558 | + send_csw(1);
|
| | 559 | + cur_sense_data.sense_key = SENSE_MEDIUM_ERROR;
|
| | 560 | + cur_sense_data.asc = ASC_WRITE_ERROR;
|
| | 561 | + cur_sense_data.ascq = 0;
|
| | 562 | + }
|
| | 563 | + send_csw(0);
|
| | 564 | + write_rc = 0;
|
| | 565 | + }
|
| | 566 | + else receive_block_data(umsbuf[1][!writebuf_current], MIN(UMS_BUFSIZE, remaining));
|
| | 567 | +}
|
| | 568 | +
|
| | 569 | +
|
| | 570 | +void ums_xfer_complete(bool in, int bytesleft)
|
| | 571 | +{
|
| | 572 | + length -= bytesleft;
|
| | 573 | + switch (state)
|
| | 574 | + {
|
| | 575 | + case RECEIVING_BLOCKS:
|
| | 576 | + if (length != storage_info.sector_size * cur_cmd.count && length != UMS_BUFSIZE) break;
|
| | 577 | + update_readcache();
|
| | 578 | + if (!writebuf_busy) writebuf_push();
|
| | 579 | + else writebuf_overrun = true;
|
| | 580 | + enqueue_async();
|
| | 581 | + break;
|
| | 582 | + case WAITING_FOR_CSW_COMPLETION:
|
| | 583 | + state = WAITING_FOR_COMMAND;
|
| | 584 | + listen();
|
| | 585 | + break;
|
| | 586 | + case WAITING_FOR_COMMAND:
|
| | 587 | + invalidate_dcache(&cmdbuf, sizeof(cmdbuf)); // Who pulls this into a cache line!?
|
| | 588 | + handle_scsi(&cmdbuf.cbw);
|
| | 589 | + break;
|
| | 590 | + case SENDING_RESULT:
|
| | 591 | + send_csw(0);
|
| | 592 | + break;
|
| | 593 | + case SENDING_FAILED_RESULT:
|
| | 594 | + send_csw(1);
|
| | 595 | + break;
|
| | 596 | + case SENDING_BLOCKS:
|
| | 597 | + if (!cur_cmd.count) send_csw(0);
|
| | 598 | + else send_and_read_next();
|
| | 599 | + break;
|
| | 600 | + }
|
| | 601 | +}
|
| | 602 | +
|
| | 603 | +void ums_handle_async()
|
| | 604 | +{
|
| | 605 | + if (writebuf_overrun)
|
| | 606 | + {
|
| | 607 | + writebuf_overrun = false;
|
| | 608 | + writebuf_push();
|
| | 609 | + }
|
| | 610 | + if (writebuf_busy)
|
| | 611 | + {
|
| | 612 | + write_rc |= storage_write_sectors_md(0, writebuf_sector, writebuf_count, umsbuf[1][writebuf_current]);
|
| | 613 | + writebuf_busy = false;
|
| | 614 | + }
|
| | 615 | +
|
| | 616 | + int c = readbuf_current;
|
| | 617 | + if (!readbuf_count[c])
|
| | 618 | + {
|
| | 619 | + int sector = readbuf_sector[c];
|
| | 620 | + int count = MIN(UMS_BUFSIZE / storage_info.sector_size, storage_info.num_sectors - sector);
|
| | 621 | + if (IS_ERR(storage_read_sectors_md(0, sector, count, umsbuf[0][c]))) count = -1;
|
| | 622 | + if (sector == readbuf_sector[c]) readbuf_count[c] = count;
|
| | 623 | + if (sector != readbuf_sector[c]) readbuf_count[c] = 0; // Extremely ugly hack to neutralize race condition
|
| | 624 | + }
|
| | 625 | + if (read_blocked)
|
| | 626 | + {
|
| | 627 | + read_blocked = false;
|
| | 628 | + if (readbuf_count[readbuf_current] < 0)
|
| | 629 | + {
|
| | 630 | + send_csw(1);
|
| | 631 | + cur_sense_data.sense_key = SENSE_MEDIUM_ERROR;
|
| | 632 | + cur_sense_data.asc = ASC_READ_ERROR;
|
| | 633 | + cur_sense_data.ascq = 0;
|
| | 634 | + }
|
| | 635 | + else send_and_read_next();
|
| | 636 | + }
|
| | 637 | +}
|
| | 638 | +
|
| Index: apps/diskmode/scsi.h |
| — | — | @@ -0,0 +1,152 @@ |
| | 2 | +//
|
| | 3 | +//
|
| | 4 | +// Copyright 2013 TheSeven
|
| | 5 | +// Copyright 2014 user890104
|
| | 6 | +//
|
| | 7 | +//
|
| | 8 | +// This file is part of emCORE.
|
| | 9 | +//
|
| | 10 | +// emCORE is free software: you can redistribute it and/or
|
| | 11 | +// modify it under the terms of the GNU General Public License as
|
| | 12 | +// published by the Free Software Foundation, either version 2 of the
|
| | 13 | +// License, or (at your option) any later version.
|
| | 14 | +//
|
| | 15 | +// emCORE is distributed in the hope that it will be useful,
|
| | 16 | +// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
| | 17 | +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
| | 18 | +// See the GNU General Public License for more details.
|
| | 19 | +//
|
| | 20 | +// You should have received a copy of the GNU General Public License along
|
| | 21 | +// with emCORE. If not, see <http://www.gnu.org/licenses/>.
|
| | 22 | +//
|
| | 23 | +//
|
| | 24 | +
|
| | 25 | +
|
| | 26 | +#ifndef __SCSI_H__
|
| | 27 | +#define __SCSI_H__
|
| | 28 | +
|
| | 29 | +#include "emcoreapp.h"
|
| | 30 | +
|
| | 31 | +
|
| | 32 | +#define SCSI_TEST_UNIT_READY 0x00
|
| | 33 | +#define SCSI_INQUIRY 0x12
|
| | 34 | +#define SCSI_MODE_SENSE_6 0x1a
|
| | 35 | +#define SCSI_MODE_SENSE_10 0x5a
|
| | 36 | +#define SCSI_REQUEST_SENSE 0x03
|
| | 37 | +#define SCSI_ALLOW_MEDIUM_REMOVAL 0x1e
|
| | 38 | +#define SCSI_READ_CAPACITY 0x25
|
| | 39 | +#define SCSI_READ_FORMAT_CAPACITY 0x23
|
| | 40 | +#define SCSI_READ_10 0x28
|
| | 41 | +#define SCSI_WRITE_10 0x2a
|
| | 42 | +#define SCSI_START_STOP_UNIT 0x1b
|
| | 43 | +#define SCSI_REPORT_LUNS 0xa0
|
| | 44 | +#define SCSI_WRITE_BUFFER 0x3b
|
| | 45 | +
|
| | 46 | +#define SENSE_NOT_READY 0x02
|
| | 47 | +#define SENSE_MEDIUM_ERROR 0x03
|
| | 48 | +#define SENSE_ILLEGAL_REQUEST 0x05
|
| | 49 | +#define SENSE_UNIT_ATTENTION 0x06
|
| | 50 | +
|
| | 51 | +#define ASC_MEDIUM_NOT_PRESENT 0x3a
|
| | 52 | +#define ASC_INVALID_FIELD_IN_CBD 0x24
|
| | 53 | +#define ASC_LBA_OUT_OF_RANGE 0x21
|
| | 54 | +#define ASC_WRITE_ERROR 0x0C
|
| | 55 | +#define ASC_READ_ERROR 0x11
|
| | 56 | +#define ASC_NOT_READY 0x04
|
| | 57 | +#define ASC_INVALID_COMMAND 0x20
|
| | 58 | +
|
| | 59 | +#define ASCQ_BECOMING_READY 0x01
|
| | 60 | +
|
| | 61 | +#define DIRECT_ACCESS_DEVICE 0x00
|
| | 62 | +#define DEVICE_REMOVABLE 0x80
|
| | 63 | +
|
| | 64 | +#define SCSI_FORMAT_CAPACITY_FORMATTED_MEDIA 0x02000000
|
| | 65 | +
|
| | 66 | +
|
| | 67 | +struct __attribute__((packed)) inquiry_data
|
| | 68 | +{
|
| | 69 | + unsigned char DeviceType;
|
| | 70 | + unsigned char DeviceTypeModifier;
|
| | 71 | + unsigned char Versions;
|
| | 72 | + unsigned char Format;
|
| | 73 | + unsigned char AdditionalLength;
|
| | 74 | + unsigned char Reserved[2];
|
| | 75 | + unsigned char Capability;
|
| | 76 | + char VendorId[8];
|
| | 77 | + char ProductId[16];
|
| | 78 | + char ProductRevisionLevel[4];
|
| | 79 | +};
|
| | 80 | +
|
| | 81 | +struct __attribute__((packed)) report_lun_data
|
| | 82 | +{
|
| | 83 | + unsigned int lun_list_length;
|
| | 84 | + unsigned int reserved1;
|
| | 85 | + unsigned char luns[1][8];
|
| | 86 | +};
|
| | 87 | +
|
| | 88 | +struct __attribute__((packed)) sense_data
|
| | 89 | +{
|
| | 90 | + unsigned char ResponseCode;
|
| | 91 | + unsigned char Obsolete;
|
| | 92 | + unsigned char fei_sensekey;
|
| | 93 | + unsigned int Information;
|
| | 94 | + unsigned char AdditionalSenseLength;
|
| | 95 | + unsigned int CommandSpecificInformation;
|
| | 96 | + unsigned char AdditionalSenseCode;
|
| | 97 | + unsigned char AdditionalSenseCodeQualifier;
|
| | 98 | + unsigned char FieldReplaceableUnitCode;
|
| | 99 | + unsigned char SKSV;
|
| | 100 | + unsigned short SenseKeySpecific;
|
| | 101 | +};
|
| | 102 | +
|
| | 103 | +struct __attribute__((packed)) mode_sense_bdesc_longlba
|
| | 104 | +{
|
| | 105 | + unsigned char num_blocks[8];
|
| | 106 | + unsigned char reserved[4];
|
| | 107 | + unsigned char block_size[4];
|
| | 108 | +};
|
| | 109 | +
|
| | 110 | +struct __attribute__((packed)) mode_sense_bdesc_shortlba
|
| | 111 | +{
|
| | 112 | + unsigned char density_code;
|
| | 113 | + unsigned char num_blocks[3];
|
| | 114 | + unsigned char reserved;
|
| | 115 | + unsigned char block_size[3];
|
| | 116 | +};
|
| | 117 | +
|
| | 118 | +struct __attribute__((packed)) mode_sense_data_10
|
| | 119 | +{
|
| | 120 | + unsigned short mode_data_length;
|
| | 121 | + unsigned char medium_type;
|
| | 122 | + unsigned char device_specific;
|
| | 123 | + unsigned char longlba;
|
| | 124 | + unsigned char reserved;
|
| | 125 | + unsigned short block_descriptor_length;
|
| | 126 | + struct mode_sense_bdesc_longlba block_descriptor;
|
| | 127 | +};
|
| | 128 | +
|
| | 129 | +struct __attribute__((packed)) mode_sense_data_6
|
| | 130 | +{
|
| | 131 | + unsigned char mode_data_length;
|
| | 132 | + unsigned char medium_type;
|
| | 133 | + unsigned char device_specific;
|
| | 134 | + unsigned char block_descriptor_length;
|
| | 135 | + struct mode_sense_bdesc_shortlba block_descriptor;
|
| | 136 | +};
|
| | 137 | +
|
| | 138 | +struct __attribute__((packed)) capacity
|
| | 139 | +{
|
| | 140 | + unsigned int block_count;
|
| | 141 | + unsigned int block_size;
|
| | 142 | +};
|
| | 143 | +
|
| | 144 | +struct __attribute__((packed)) format_capacity
|
| | 145 | +{
|
| | 146 | + unsigned int following_length;
|
| | 147 | + unsigned int block_count;
|
| | 148 | + unsigned int block_size;
|
| | 149 | +};
|
| | 150 | +
|
| | 151 | +
|
| | 152 | +#endif
|
| | 153 | +
|
| Index: apps/diskmode/main.h |
| — | — | @@ -0,0 +1,33 @@ |
| | 2 | +//
|
| | 3 | +//
|
| | 4 | +// Copyright 2013 TheSeven
|
| | 5 | +// Copyright 2014 user890104
|
| | 6 | +//
|
| | 7 | +//
|
| | 8 | +// This file is part of emCORE.
|
| | 9 | +//
|
| | 10 | +// emCORE is free software: you can redistribute it and/or
|
| | 11 | +// modify it under the terms of the GNU General Public License as
|
| | 12 | +// published by the Free Software Foundation, either version 2 of the
|
| | 13 | +// License, or (at your option) any later version.
|
| | 14 | +//
|
| | 15 | +// emCORE is distributed in the hope that it will be useful,
|
| | 16 | +// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
| | 17 | +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
| | 18 | +// See the GNU General Public License for more details.
|
| | 19 | +//
|
| | 20 | +// You should have received a copy of the GNU General Public License along
|
| | 21 | +// with emCORE. If not, see <http://www.gnu.org/licenses/>.
|
| | 22 | +//
|
| | 23 | +//
|
| | 24 | +
|
| | 25 | +
|
| | 26 | +#ifndef __MAIN_H__
|
| | 27 | +#define __MAIN_H__
|
| | 28 | +
|
| | 29 | +#include "emcoreapp.h"
|
| | 30 | +
|
| | 31 | +
|
| | 32 | +
|
| | 33 | +#endif
|
| | 34 | +
|
| Index: apps/diskmode/ums.h |
| — | — | @@ -0,0 +1,39 @@ |
| | 2 | +//
|
| | 3 | +//
|
| | 4 | +// Copyright 2013 TheSeven
|
| | 5 | +// Copyright 2014 user890104
|
| | 6 | +//
|
| | 7 | +//
|
| | 8 | +// This file is part of emCORE.
|
| | 9 | +//
|
| | 10 | +// emCORE is free software: you can redistribute it and/or
|
| | 11 | +// modify it under the terms of the GNU General Public License as
|
| | 12 | +// published by the Free Software Foundation, either version 2 of the
|
| | 13 | +// License, or (at your option) any later version.
|
| | 14 | +//
|
| | 15 | +// emCORE is distributed in the hope that it will be useful,
|
| | 16 | +// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
| | 17 | +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
| | 18 | +// See the GNU General Public License for more details.
|
| | 19 | +//
|
| | 20 | +// You should have received a copy of the GNU General Public License along
|
| | 21 | +// with emCORE. If not, see <http://www.gnu.org/licenses/>.
|
| | 22 | +//
|
| | 23 | +//
|
| | 24 | +
|
| | 25 | +
|
| | 26 | +#ifndef __UMS_H__
|
| | 27 | +#define __UMS_H__
|
| | 28 | +
|
| | 29 | +#include "emcoreapp.h"
|
| | 30 | +
|
| | 31 | +
|
| | 32 | +extern bool ums_ejected;
|
| | 33 | +
|
| | 34 | +
|
| | 35 | +extern void ums_init();
|
| | 36 | +extern void ums_listen();
|
| | 37 | +
|
| | 38 | +
|
| | 39 | +#endif
|
| | 40 | +
|
| Index: apps/diskmode/usb.c |
| — | — | @@ -0,0 +1,358 @@ |
| | 2 | +//
|
| | 3 | +//
|
| | 4 | +// Copyright 2013 TheSeven
|
| | 5 | +// Copyright 2014 user890104
|
| | 6 | +//
|
| | 7 | +//
|
| | 8 | +// This file is part of emCORE.
|
| | 9 | +//
|
| | 10 | +// emCORE is free software: you can redistribute it and/or
|
| | 11 | +// modify it under the terms of the GNU General Public License as
|
| | 12 | +// published by the Free Software Foundation, either version 2 of the
|
| | 13 | +// License, or (at your option) any later version.
|
| | 14 | +//
|
| | 15 | +// emCORE is distributed in the hope that it will be useful,
|
| | 16 | +// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
| | 17 | +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
| | 18 | +// See the GNU General Public License for more details.
|
| | 19 | +//
|
| | 20 | +// You should have received a copy of the GNU General Public License along
|
| | 21 | +// with emCORE. If not, see <http://www.gnu.org/licenses/>.
|
| | 22 | +//
|
| | 23 | +//
|
| | 24 | +
|
| | 25 | +
|
| | 26 | +#include "emcoreapp.h"
|
| | 27 | +#include "usb.h"
|
| | 28 | +#include "ums.h"
|
| | 29 | +
|
| | 30 | +
|
| | 31 | +int usb_maxlen;
|
| | 32 | +
|
| | 33 | +static const struct usb_instance* usb_handle;
|
| | 34 | +static union usb_endpoint_number usb_outep = { .number = 0, .direction = USB_ENDPOINT_DIRECTION_OUT };
|
| | 35 | +static union usb_endpoint_number usb_inep = { .number = 0, .direction = USB_ENDPOINT_DIRECTION_IN };
|
| | 36 | +static int maxpacket = 512;
|
| | 37 | +static struct wakeup mainloop_wakeup;
|
| | 38 | +
|
| | 39 | +
|
| | 40 | +static const struct usb_devicedescriptor usb_devicedescriptor =
|
| | 41 | +{
|
| | 42 | + .bLength = sizeof(struct usb_devicedescriptor),
|
| | 43 | + .bDescriptorType = USB_DESCRIPTOR_TYPE_DEVICE,
|
| | 44 | + .bcdUSB = 0x0200,
|
| | 45 | + .bDeviceClass = 0,
|
| | 46 | + .bDeviceSubClass = 0,
|
| | 47 | + .bDeviceProtocol = 0,
|
| | 48 | + .bMaxPacketSize0 = 64,
|
| | 49 | + .idVendor = 0xffff,
|
| | 50 | + .idProduct = 0xe001,
|
| | 51 | + .bcdDevice = 2,
|
| | 52 | + .iManufacturer = 1,
|
| | 53 | + .iProduct = 2,
|
| | 54 | + .iSerialNumber = 0,
|
| | 55 | + .bNumConfigurations = 1,
|
| | 56 | +};
|
| | 57 | +
|
| | 58 | +
|
| | 59 | +static struct __attribute__((packed)) _usb_config1_descriptors
|
| | 60 | +{
|
| | 61 | + struct usb_configurationdescriptor c1;
|
| | 62 | + struct usb_interfacedescriptor c1_i0_a0;
|
| | 63 | + struct usb_endpointdescriptor c1_i0_a0_e1out;
|
| | 64 | + struct usb_endpointdescriptor c1_i0_a0_e1in;
|
| | 65 | +} usb_config1_descriptors =
|
| | 66 | +{
|
| | 67 | + .c1 =
|
| | 68 | + {
|
| | 69 | + .bLength = sizeof(struct usb_configurationdescriptor),
|
| | 70 | + .bDescriptorType = USB_DESCRIPTOR_TYPE_CONFIGURATION,
|
| | 71 | + .wTotalLength = sizeof(struct _usb_config1_descriptors),
|
| | 72 | + .bNumInterfaces = 1,
|
| | 73 | + .bConfigurationValue = 1,
|
| | 74 | + .iConfiguration = 0,
|
| | 75 | + .bmAttributes = { .buspowered = 1, .selfpowered = 1 },
|
| | 76 | + .bMaxPower = 100 / 2,
|
| | 77 | + },
|
| | 78 | + .c1_i0_a0 =
|
| | 79 | + {
|
| | 80 | + .bLength = sizeof(struct usb_interfacedescriptor),
|
| | 81 | + .bDescriptorType = USB_DESCRIPTOR_TYPE_INTERFACE,
|
| | 82 | + .bInterfaceNumber = 0,
|
| | 83 | + .bAlternateSetting = 0,
|
| | 84 | + .bNumEndpoints = 2,
|
| | 85 | + .bInterfaceClass = 0x08,
|
| | 86 | + .bInterfaceSubClass = 0x06,
|
| | 87 | + .bInterfaceProtocol = 0x50,
|
| | 88 | + .iInterface = 0,
|
| | 89 | + },
|
| | 90 | + .c1_i0_a0_e1out =
|
| | 91 | + {
|
| | 92 | + .bLength = sizeof(struct usb_endpointdescriptor),
|
| | 93 | + .bDescriptorType = USB_DESCRIPTOR_TYPE_ENDPOINT,
|
| | 94 | + .bEndpointAddress = { .number = 0, .direction = USB_ENDPOINT_DIRECTION_OUT },
|
| | 95 | + .bmAttributes = { .type = USB_ENDPOINT_ATTRIBUTE_TYPE_BULK },
|
| | 96 | + .wMaxPacketSize = 512,
|
| | 97 | + .bInterval = 1,
|
| | 98 | + },
|
| | 99 | + .c1_i0_a0_e1in =
|
| | 100 | + {
|
| | 101 | + .bLength = sizeof(struct usb_endpointdescriptor),
|
| | 102 | + .bDescriptorType = USB_DESCRIPTOR_TYPE_ENDPOINT,
|
| | 103 | + .bEndpointAddress = { .number = 0, .direction = USB_ENDPOINT_DIRECTION_IN },
|
| | 104 | + .bmAttributes = { .type = USB_ENDPOINT_ATTRIBUTE_TYPE_BULK },
|
| | 105 | + .wMaxPacketSize = 512,
|
| | 106 | + .bInterval = 1,
|
| | 107 | + },
|
| | 108 | +};
|
| | 109 | +
|
| | 110 | +
|
| | 111 | +static const struct usb_stringdescriptor usb_string_language =
|
| | 112 | +{
|
| | 113 | + .bLength = sizeof(usb_string_language) + sizeof(*usb_string_language.wString),
|
| | 114 | + .bDescriptorType = USB_DESCRIPTOR_TYPE_STRING,
|
| | 115 | + .wString = { 0x0409 },
|
| | 116 | +};
|
| | 117 | +
|
| | 118 | +
|
| | 119 | +static const struct usb_stringdescriptor usb_string_vendor =
|
| | 120 | +{
|
| | 121 | + .bLength = sizeof(usb_string_vendor) + sizeof(*usb_string_vendor.wString) * 14,
|
| | 122 | + .bDescriptorType = USB_DESCRIPTOR_TYPE_STRING,
|
| | 123 | + .wString = { 'f', 'r', 'e', 'e', 'm', 'y', 'i', 'p', 'o', 'd', '.', 'o', 'r', 'g' },
|
| | 124 | +};
|
| | 125 | +
|
| | 126 | +
|
| | 127 | +static const struct usb_stringdescriptor usb_string_product =
|
| | 128 | +{
|
| | 129 | + .bLength = sizeof(usb_string_product) + sizeof(*usb_string_product.wString) * 16,
|
| | 130 | + .bDescriptorType = USB_DESCRIPTOR_TYPE_STRING,
|
| | 131 | + .wString = { 'e', 'm', 'C', 'O', 'R', 'E', ' ', 'D', 'i', 's', 'k', ' ', 'M', 'o', 'd', 'e' },
|
| | 132 | +};
|
| | 133 | +
|
| | 134 | +
|
| | 135 | +static const struct usb_stringdescriptor* usb_stringdescriptors[] =
|
| | 136 | +{
|
| | 137 | + &usb_string_language,
|
| | 138 | + &usb_string_vendor,
|
| | 139 | + &usb_string_product,
|
| | 140 | +};
|
| | 141 | +
|
| | 142 | +
|
| | 143 | +int handle_ctrl_request(const struct usb_instance* data, int interface, union usb_ep0_buffer* request, const void** response)
|
| | 144 | +{
|
| | 145 | + int size = -1;
|
| | 146 | + switch (request->setup.bmRequestType.type)
|
| | 147 | + {
|
| | 148 | + case USB_SETUP_BMREQUESTTYPE_TYPE_CLASS:
|
| | 149 | + switch (request->setup.bRequest.raw)
|
| | 150 | + {
|
| | 151 | + case 0xfe: // GET_MAX_LUN
|
| | 152 | + data->buffer->raw[0] = 0;
|
| | 153 | + size = 1;
|
| | 154 | + break;
|
| | 155 | + case 0xff: // STORAGE_RESET
|
| | 156 | + size = 0;
|
| | 157 | + break;
|
| | 158 | + default: break;
|
| | 159 | + }
|
| | 160 | + break;
|
| | 161 | + default: break;
|
| | 162 | + }
|
| | 163 | + return size;
|
| | 164 | +}
|
| | 165 | +
|
| | 166 | +
|
| | 167 | +void handle_set_altsetting(const struct usb_instance* data, int interface, int altsetting)
|
| | 168 | +{
|
| | 169 | + usb_handle = data;
|
| | 170 | + usb_maxlen = maxpacket * MIN(usb_get_max_transfer_size(usb_handle, usb_outep),
|
| | 171 | + usb_get_max_transfer_size(usb_handle, usb_inep));
|
| | 172 | + usb_configure_ep(usb_handle, usb_outep, USB_ENDPOINT_TYPE_BULK, maxpacket);
|
| | 173 | + usb_configure_ep(usb_handle, usb_inep, USB_ENDPOINT_TYPE_BULK, maxpacket);
|
| | 174 | + ums_listen();
|
| | 175 | +}
|
| | 176 | +
|
| | 177 | +
|
| | 178 | +void handle_unset_altsetting(const struct usb_instance* data, int interface, int altsetting)
|
| | 179 | +{
|
| | 180 | + usb_unconfigure_ep(usb_handle, usb_outep);
|
| | 181 | + usb_unconfigure_ep(usb_handle, usb_inep);
|
| | 182 | +}
|
| | 183 | +
|
| | 184 | +
|
| | 185 | +int handle_ep_ctrl_request(const struct usb_instance* data, int interface, int endpoint, union usb_ep0_buffer* request, const void** response)
|
| | 186 | +{
|
| | 187 | + int size = -1;
|
| | 188 | + switch (request->setup.bmRequestType.type)
|
| | 189 | + {
|
| | 190 | + case USB_SETUP_BMREQUESTTYPE_TYPE_STANDARD:
|
| | 191 | + switch (request->setup.bRequest.req)
|
| | 192 | + {
|
| | 193 | + case USB_SETUP_BREQUEST_CLEAR_FEATURE:
|
| | 194 | + if (request->setup.wLength || request->setup.wValue) break;
|
| | 195 | + ums_listen();
|
| | 196 | + break;
|
| | 197 | + default: break;
|
| | 198 | + }
|
| | 199 | + break;
|
| | 200 | + default: break;
|
| | 201 | + }
|
| | 202 | + return size;
|
| | 203 | +}
|
| | 204 | +
|
| | 205 | +
|
| | 206 | +void handle_xfer_complete(const struct usb_instance* data, int ifnum, int epnum, int bytesleft)
|
| | 207 | +{
|
| | 208 | + ums_xfer_complete(epnum & 0x80, bytesleft);
|
| | 209 | +}
|
| | 210 | +
|
| | 211 | +
|
| | 212 | +void handle_timeout(const struct usb_instance* data, int interface, int endpoint, int bytesleft)
|
| | 213 | +{
|
| | 214 | + usb_set_stall(usb_handle, usb_outep, true);
|
| | 215 | + usb_set_stall(usb_handle, usb_inep, true);
|
| | 216 | +}
|
| | 217 | +
|
| | 218 | +
|
| | 219 | +static void handle_bus_reset(const struct usb_instance* data, int configuration, int interface, int highspeed)
|
| | 220 | +{
|
| | 221 | + maxpacket = highspeed ? 512 : 64;
|
| | 222 | + usb_config1_descriptors.c1_i0_a0_e1out.wMaxPacketSize = maxpacket;
|
| | 223 | + usb_config1_descriptors.c1_i0_a0_e1in.wMaxPacketSize = maxpacket;
|
| | 224 | +}
|
| | 225 | +
|
| | 226 | +
|
| | 227 | +static struct usb_endpoint usb_c1_i0_a0_ep1out =
|
| | 228 | +{
|
| | 229 | + .number = { .number = 0, .direction = USB_ENDPOINT_DIRECTION_OUT },
|
| | 230 | + .ctrl_request = handle_ep_ctrl_request,
|
| | 231 | + .xfer_complete = handle_xfer_complete,
|
| | 232 | +};
|
| | 233 | +
|
| | 234 | +
|
| | 235 | +static struct usb_endpoint usb_c1_i0_a0_ep1in =
|
| | 236 | +{
|
| | 237 | + .number = { .number = 0, .direction = USB_ENDPOINT_DIRECTION_IN },
|
| | 238 | + .ctrl_request = handle_ep_ctrl_request,
|
| | 239 | + .xfer_complete = handle_xfer_complete,
|
| | 240 | + .timeout = handle_timeout,
|
| | 241 | +};
|
| | 242 | +
|
| | 243 | +
|
| | 244 | +static const struct usb_altsetting usb_c1_i0_a0 =
|
| | 245 | +{
|
| | 246 | + .set_altsetting = handle_set_altsetting,
|
| | 247 | + .unset_altsetting = handle_unset_altsetting,
|
| | 248 | + .endpoint_count = 2,
|
| | 249 | + .endpoints =
|
| | 250 | + {
|
| | 251 | + &usb_c1_i0_a0_ep1out,
|
| | 252 | + &usb_c1_i0_a0_ep1in,
|
| | 253 | + },
|
| | 254 | +};
|
| | 255 | +
|
| | 256 | +
|
| | 257 | +static const struct usb_interface usb_c1_i0 =
|
| | 258 | +{
|
| | 259 | + .bus_reset = handle_bus_reset,
|
| | 260 | + .ctrl_request = handle_ctrl_request,
|
| | 261 | + .altsetting_count = 1,
|
| | 262 | + .altsettings =
|
| | 263 | + {
|
| | 264 | + &usb_c1_i0_a0,
|
| | 265 | + },
|
| | 266 | +};
|
| | 267 | +
|
| | 268 | +
|
| | 269 | +static const struct usb_configuration usb_c1 =
|
| | 270 | +{
|
| | 271 | + .descriptor = &usb_config1_descriptors.c1,
|
| | 272 | + .set_configuration = NULL,
|
| | 273 | + .unset_configuration = NULL,
|
| | 274 | + .interface_count = 1,
|
| | 275 | + .interfaces =
|
| | 276 | + {
|
| | 277 | + &usb_c1_i0,
|
| | 278 | + },
|
| | 279 | +};
|
| | 280 | +
|
| | 281 | +
|
| | 282 | +static const struct usb_configuration* usb_configurations[] =
|
| | 283 | +{
|
| | 284 | + &usb_c1,
|
| | 285 | +};
|
| | 286 | +
|
| | 287 | +
|
| | 288 | +void usb_prepare()
|
| | 289 | +{
|
| | 290 | + int i;
|
| | 291 | + uint32_t eps = usbmanager_get_available_endpoints();
|
| | 292 | + int ep_out = 0;
|
| | 293 | + int ep_in = 0;
|
| | 294 | + for (i = 1; i < 16; i++)
|
| | 295 | + if (eps & (1 << i))
|
| | 296 | + {
|
| | 297 | + ep_out = i;
|
| | 298 | + break;
|
| | 299 | + }
|
| | 300 | + for (i = 1; i < 16; i++)
|
| | 301 | + if (eps & (1 << (16 + i)))
|
| | 302 | + {
|
| | 303 | + ep_in = i;
|
| | 304 | + break;
|
| | 305 | + }
|
| | 306 | + if (!ep_out || !ep_in) panicf(PANIC_KILLTHREAD, "Not enough USB endpoints available!\n");
|
| | 307 | + usb_config1_descriptors.c1_i0_a0_e1out.bEndpointAddress.number = ep_out;
|
| | 308 | + usb_config1_descriptors.c1_i0_a0_e1in.bEndpointAddress.number = ep_in;
|
| | 309 | + usb_c1_i0_a0_ep1out.number.number = ep_out;
|
| | 310 | + usb_c1_i0_a0_ep1in.number.number = ep_in;
|
| | 311 | + usb_outep.number = ep_out;
|
| | 312 | + usb_inep.number = ep_in;
|
| | 313 | +}
|
| | 314 | +
|
| | 315 | +
|
| | 316 | +void usb_connect()
|
| | 317 | +{
|
| | 318 | + int rc = usbmanager_install_custom(&usb_devicedescriptor, ARRAYLEN(usb_configurations), usb_configurations,
|
| | 319 | + ARRAYLEN(usb_stringdescriptors), usb_stringdescriptors, true);
|
| | 320 | + if (IS_ERR(rc))
|
| | 321 | + {
|
| | 322 | + cprintf(3, "Failed to register USB handler: %08X\n", rc);
|
| | 323 | + return;
|
| | 324 | + }
|
| | 325 | +
|
| | 326 | + wakeup_init(&mainloop_wakeup);
|
| | 327 | +
|
| | 328 | + while (!ums_ejected && usbmanager_get_connected())
|
| | 329 | + if (wakeup_wait(&mainloop_wakeup, 200000) == THREAD_OK)
|
| | 330 | + ums_handle_async();
|
| | 331 | +
|
| | 332 | + usbmanager_uninstall_custom();
|
| | 333 | +}
|
| | 334 | +
|
| | 335 | +
|
| | 336 | +void enqueue_async()
|
| | 337 | +{
|
| | 338 | + wakeup_signal(&mainloop_wakeup);
|
| | 339 | +}
|
| | 340 | +
|
| | 341 | +
|
| | 342 | +void usb_transmit(void* buffer, uint32_t len)
|
| | 343 | +{
|
| | 344 | + usb_start_tx(usb_handle, usb_inep, buffer, len);
|
| | 345 | +}
|
| | 346 | +
|
| | 347 | +
|
| | 348 | +void usb_receive(void* buffer, uint32_t len)
|
| | 349 | +{
|
| | 350 | + usb_start_rx(usb_handle, usb_outep, buffer, len);
|
| | 351 | +}
|
| | 352 | +
|
| | 353 | +
|
| | 354 | +void usb_stall()
|
| | 355 | +{
|
| | 356 | + usb_set_stall(usb_handle, usb_outep, true);
|
| | 357 | + usb_set_stall(usb_handle, usb_inep, true);
|
| | 358 | +}
|
| | 359 | +
|
| Index: apps/diskmode/usb.h |
| — | — | @@ -0,0 +1,42 @@ |
| | 2 | +//
|
| | 3 | +//
|
| | 4 | +// Copyright 2013 TheSeven
|
| | 5 | +// Copyright 2014 user890104
|
| | 6 | +//
|
| | 7 | +//
|
| | 8 | +// This file is part of emCORE.
|
| | 9 | +//
|
| | 10 | +// emCORE is free software: you can redistribute it and/or
|
| | 11 | +// modify it under the terms of the GNU General Public License as
|
| | 12 | +// published by the Free Software Foundation, either version 2 of the
|
| | 13 | +// License, or (at your option) any later version.
|
| | 14 | +//
|
| | 15 | +// emCORE is distributed in the hope that it will be useful,
|
| | 16 | +// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
| | 17 | +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
| | 18 | +// See the GNU General Public License for more details.
|
| | 19 | +//
|
| | 20 | +// You should have received a copy of the GNU General Public License along
|
| | 21 | +// with emCORE. If not, see <http://www.gnu.org/licenses/>.
|
| | 22 | +//
|
| | 23 | +//
|
| | 24 | +
|
| | 25 | +
|
| | 26 | +#ifndef __USB_H__
|
| | 27 | +#define __USB_H__
|
| | 28 | +
|
| | 29 | +#include "emcoreapp.h"
|
| | 30 | +
|
| | 31 | +
|
| | 32 | +extern int usb_maxlen;
|
| | 33 | +
|
| | 34 | +
|
| | 35 | +extern void usb_prepare();
|
| | 36 | +extern void usb_connect();
|
| | 37 | +extern void enqueue_async();
|
| | 38 | +extern void usb_transmit(void* buffer, uint32_t len);
|
| | 39 | +extern void usb_receive(void* buffer, uint32_t len);
|
| | 40 | +extern void usb_stall();
|
| | 41 | +
|
| | 42 | +#endif
|
| | 43 | +
|
