freemyipod r924 - Code Review

Jump to: navigation, search
Repository:freemyipod
Revision:r923‎ | r924 | r925 >
Date:21:43, 6 May 2014
Author:theseven
Status:new
Tags:
Comment:
diskmode: Split the code into multiple files, add some buffering and I/O error handling
Modified paths:
  • /apps/diskmode/SOURCES (modified) (history)
  • /apps/diskmode/main.c (modified) (history)
  • /apps/diskmode/main.h (added) (history)
  • /apps/diskmode/scsi.h (added) (history)
  • /apps/diskmode/ums.c (added) (history)
  • /apps/diskmode/ums.h (added) (history)
  • /apps/diskmode/usb.c (added) (history)
  • /apps/diskmode/usb.h (added) (history)

Diff [purge]

Index: apps/diskmode/SOURCES
@@ -1 +1,3 @@
22 main.c
 3+usb.c
 4+ums.c
Index: apps/diskmode/main.c
@@ -22,941 +22,43 @@
2323 //
2424
2525
26 -#define LITTLE_ENDIAN
27 -
28 -
2926 #include "emcoreapp.h"
3027 #include "libboot.h"
 28+#include "ums.h"
 29+#include "usb.h"
3130
3231
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 -
86432 static void main(int argc, const char** argv)
86533 {
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();
86840
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 -
87441 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();
90543
906 - wakeup_init(&pending_op_wakeup);
 44+ cprintf(3, "Disk was ejected successfully.\nReturning to the bootmenu...\n\n");
90745
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");
95049 struct libboot_api* boot = (struct libboot_api*)libboot->api;
95150
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)
95555 panicf(PANIC_KILLTHREAD, "Unable to start the bootmenu! Press MENU+SELECT to reboot your device.\n");
956 - }
 56+
 57+ release_library(libboot);
95758
95859 disk_mount(0);
959 - execimage(bootinfo.app, false, bootinfo.argc, bootinfo.argv);
 60+ execimage(app, false, 0, NULL);
96061 }
96162
96263
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+