freemyipod r967 - Code Review

Jump to: navigation, search
Repository:freemyipod
Revision:< r966‎ | r967 | r968 >
Date:23:57, 20 July 2014
Author:theseven
Status:new
Tags:
Comment:
emCORE disk mode: Add SAT-2 ATA passthrough SCSI command support
This enables smartmontools to access the drive's SMART information.
This commit also fixes some other bugs and generally improves the code.
Modified paths:
  • /apps/diskmode/scsi.h (modified) (history)
  • /apps/diskmode/ums.c (modified) (history)
  • /apps/diskmode/usb.c (modified) (history)
  • /apps/diskmode/usb.h (modified) (history)

Diff [purge]

Index: apps/diskmode/usb.c
——@@ -34,6 +34,7 @@
3535 static union usb_endpoint_number usb_inep = { .number = 0, .direction = USB_ENDPOINT_DIRECTION_IN };
3636 static int maxpacket = 512;
3737 static struct wakeup mainloop_wakeup;
 38+static char* error = NULL;
3839
3940
4041 static const struct usb_devicedescriptor usb_devicedescriptor =
——@@ -204,7 +205,7 @@
205206
206207 void handle_xfer_complete(const struct usb_instance* data, int ifnum, int epnum, int bytesleft)
207208 {
208 - ums_xfer_complete(epnum & 0x80, bytesleft);
 209+ ums_xfer_complete(epnum, bytesleft);
209210 }
210211
211212
——@@ -234,7 +235,6 @@
235236 static struct usb_endpoint usb_c1_i0_a0_ep1in =
236237 {
237238 .number = { .number = 0, .direction = USB_ENDPOINT_DIRECTION_IN },
238 - .ctrl_request = handle_ep_ctrl_request,
239239 .xfer_complete = handle_xfer_complete,
240240 .timeout = handle_timeout,
241241 };
——@@ -329,6 +329,8 @@
330330 ums_handle_async();
331331
332332 usbmanager_uninstall_custom();
 333+
 334+ if (error) panic(PANIC_KILLTHREAD, error);
333335 }
334336
335337
——@@ -350,9 +352,21 @@
351353 }
352354
353355
354 -void usb_stall()
 356+void usb_stall_in()
355357 {
356 - usb_set_stall(usb_handle, usb_outep, true);
357358 usb_set_stall(usb_handle, usb_inep, true);
358359 }
359360
 361+
 362+void usb_stall_out()
 363+{
 364+ usb_set_stall(usb_handle, usb_outep, true);
 365+}
 366+
 367+
 368+void fail(char* errormsg)
 369+{
 370+ error = errormsg;
 371+ ums_ejected = true;
 372+ wakeup_signal(&mainloop_wakeup);
 373+}
Index: apps/diskmode/usb.h
——@@ -36,7 +36,9 @@
3737 extern void enqueue_async();
3838 extern void usb_transmit(void* buffer, uint32_t len);
3939 extern void usb_receive(void* buffer, uint32_t len);
40 -extern void usb_stall();
 40+extern void usb_stall_in();
 41+extern void usb_stall_out();
 42+extern void fail(char* errormsg);
4143
4244 #endif
4345
Index: apps/diskmode/ums.c
——@@ -26,6 +26,7 @@
2727 #include "ums.h"
2828 #include "scsi.h"
2929 #include "usb.h"
 30+#include "../../emcore/trunk/target/ipodclassic/storage_ata-target.h"
3031
3132
3233 #define UMS_BUFSIZE 65536
——@@ -40,6 +41,8 @@
4142 unsigned int tag;
4243 unsigned int lun;
4344 unsigned int last_result;
 45+ unsigned int bytes_pending;
 46+ bool data_direction;
4447 } cur_cmd;
4548
4649
——@@ -52,8 +55,38 @@
5356 } cur_sense_data;
5457
5558
56 -struct __attribute__((packed,aligned(32))) command_block_wrapper
 59+static enum
5760 {
 61+ SAT_PENDING_NONE = 0,
 62+ SAT_PENDING_SRST,
 63+ SAT_PENDING_HRST,
 64+ SAT_PENDING_CMD,
 65+ SAT_PENDING_READ_CDB,
 66+} sat_pending = SAT_PENDING_NONE;
 67+static struct ata_raw_cmd_t sat_command;
 68+static struct __attribute__((packed)) sat_response_information
 69+{
 70+ uint8_t descriptor_code;
 71+ uint8_t additional_length;
 72+ uint8_t extend;
 73+ uint8_t error;
 74+ uint8_t sector_count_h;
 75+ uint8_t sector_count_l;
 76+ uint8_t lba_low_h;
 77+ uint8_t lba_low_l;
 78+ uint8_t lba_mid_h;
 79+ uint8_t lba_mid_l;
 80+ uint8_t lba_high_h;
 81+ uint8_t lba_high_l;
 82+ uint8_t device;
 83+ uint8_t status;
 84+} sat_response_information;
 85+static bool have_sat_response_information = false;
 86+static bool sat_check;
 87+
 88+
 89+struct __attribute__((packed)) command_block_wrapper
 90+{
5891 unsigned int signature;
5992 unsigned int tag;
6093 unsigned int data_transfer_length;
——@@ -78,7 +111,15 @@
79112 struct inquiry_data inquiry;
80113 struct capacity capacity_data;
81114 struct format_capacity format_capacity_data;
82 - struct sense_data sense_data;
 115+ struct sense_data_fixed sense_data_fixed;
 116+ struct
 117+ {
 118+ struct sense_data_descr header;
 119+ union
 120+ {
 121+ struct sat_response_information sat_response;
 122+ } info;
 123+ } sense_data_descr;
83124 struct mode_sense_data_6 ms_data_6;
84125 struct mode_sense_data_10 ms_data_10;
85126 struct report_lun_data lun_data;
——@@ -91,9 +132,10 @@
92133 WAITING_FOR_COMMAND,
93134 SENDING_BLOCKS,
94135 SENDING_RESULT,
95 - SENDING_FAILED_RESULT,
96136 RECEIVING_BLOCKS,
97 - WAITING_FOR_CSW_COMPLETION
 137+ WAITING_FOR_CSW_COMPLETION,
 138+ RECEIVING_SAT_WRITE,
 139+ PROCESSING,
98140 } state = WAITING_FOR_COMMAND;
99141
100142 static uint32_t length;
——@@ -135,9 +177,15 @@
136178
137179 static void send_csw(int status)
138180 {
 181+ if (cur_cmd.bytes_pending)
 182+ {
 183+ if (cur_cmd.data_direction) usb_stall_in();
 184+ else usb_stall_out();
 185+ }
 186+
139187 tb.csw.signature = 0x53425355;
140188 tb.csw.tag = cur_cmd.tag;
141 - tb.csw.data_residue = 0;
 189+ tb.csw.data_residue = cur_cmd.bytes_pending;
142190 tb.csw.status = status;
143191
144192 state = WAITING_FOR_CSW_COMPLETION;
——@@ -155,32 +203,66 @@
156204
157205 static void receive_block_data(void* data, int size)
158206 {
159 - length = size;
160 - usb_receive(data, size);
161 - state = RECEIVING_BLOCKS;
 207+ if (cur_cmd.data_direction) fail("diskmode: Attempting to receive data for IN command!");
 208+ else if (size > cur_cmd.bytes_pending) fail("diskmode: Receive overrun!");
 209+ else
 210+ {
 211+ cur_cmd.bytes_pending -= size;
 212+ length = size;
 213+ usb_receive(data, size);
 214+ state = RECEIVING_BLOCKS;
 215+ }
162216 }
163217
164218
 219+static void receive_sat_write(void* data, int size)
 220+{
 221+ if (cur_cmd.data_direction) fail("diskmode: Attempting to receive data for SAT IN command!");
 222+ else if (size > cur_cmd.bytes_pending) fail("diskmode: Receive SAT overrun!");
 223+ else
 224+ {
 225+ cur_cmd.bytes_pending -= size;
 226+ length = size;
 227+ usb_receive(data, size);
 228+ state = RECEIVING_SAT_WRITE;
 229+ }
 230+}
 231+
 232+
165233 static void send_block_data(void* data, int size)
166234 {
167 - length = size;
168 - usb_transmit(data, size);
169 - state = SENDING_BLOCKS;
 235+ if (!cur_cmd.data_direction) fail("diskmode: Attempting to send data for OUT command!");
 236+ else if (size > cur_cmd.bytes_pending) fail("diskmode: Send block overrun!");
 237+ else
 238+ {
 239+ cur_cmd.bytes_pending -= size;
 240+ length = size;
 241+ usb_transmit(data, size);
 242+ state = SENDING_BLOCKS;
 243+ }
170244 }
171245
172246
173247 static void send_command_result(void* data, int size)
174248 {
175 - length = size;
176 - usb_transmit(data, size);
177 - state = SENDING_RESULT;
 249+ if (!cur_cmd.data_direction) fail("diskmode: Attempting to send result for OUT command!");
 250+ else if (size > cur_cmd.bytes_pending) fail("diskmode: Send result overrun!");
 251+ else
 252+ {
 253+ cur_cmd.bytes_pending -= size;
 254+ length = size;
 255+ usb_transmit(data, size);
 256+ state = SENDING_RESULT;
 257+ }
178258 }
179259
180260
181 -static void send_command_failed_result()
 261+static void send_command_failed_result(unsigned char key, unsigned char asc, unsigned char ascq)
182262 {
183 - usb_transmit(NULL, 0);
184 - state = SENDING_FAILED_RESULT;
 263+ send_csw(1);
 264+ cur_sense_data.sense_key = key;
 265+ cur_sense_data.asc = asc;
 266+ cur_sense_data.ascq = ascq;
185267 }
186268
187269
——@@ -255,24 +337,22 @@
256338
257339 if (cbw->signature != 0x43425355)
258340 {
259 - usb_stall();
 341+ usb_stall_in();
 342+ usb_stall_out();
260343 return;
261344 }
262345 cur_cmd.tag = cbw->tag;
263346 cur_cmd.lun = cbw->lun;
264347 cur_cmd.cur_cmd = cbw->command_block[0];
 348+ cur_cmd.bytes_pending = cbw->data_transfer_length;
 349+ if (cbw->flags & 0x80) cur_cmd.data_direction = 1;
 350+ else cur_cmd.data_direction = 0;
265351
266352 switch (cbw->command_block[0])
267353 {
268354 case SCSI_TEST_UNIT_READY:
269355 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 - }
 356+ else send_command_failed_result(SENSE_NOT_READY, ASC_MEDIUM_NOT_PRESENT, 0);
277357 break;
278358
279359 case SCSI_REPORT_LUNS:
——@@ -291,29 +371,40 @@
292372
293373 case SCSI_REQUEST_SENSE:
294374 {
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));
 375+ if (have_sat_response_information)
 376+ {
 377+ memset(&tb.sense_data_descr.header, 0, sizeof(tb.sense_data_descr.header));
 378+ tb.sense_data_descr.header.ResponseCode = 0x72;
 379+ tb.sense_data_descr.header.fei_sensekey = cur_sense_data.sense_key & 0x0f;
 380+ tb.sense_data_descr.header.AdditionalSenseCode = cur_sense_data.asc;
 381+ tb.sense_data_descr.header.AdditionalSenseCodeQualifier = cur_sense_data.ascq;
 382+ tb.sense_data_descr.header.AdditionalSenseLength = sizeof(sat_response_information);
 383+ memcpy(&tb.sense_data_descr.info.sat_response, &sat_response_information, sizeof(sat_response_information));
 384+ send_command_result(&tb.sense_data_descr, MIN(sizeof(tb.sense_data_descr.header) + sizeof(sat_response_information), length));
 385+ }
 386+ else
 387+ {
 388+ tb.sense_data_fixed.ResponseCode = 0x70;
 389+ tb.sense_data_fixed.Obsolete = 0;
 390+ tb.sense_data_fixed.fei_sensekey = cur_sense_data.sense_key & 0x0f;
 391+ tb.sense_data_fixed.Information = cur_sense_data.information;
 392+ tb.sense_data_fixed.AdditionalSenseLength = 10;
 393+ tb.sense_data_fixed.CommandSpecificInformation = 0;
 394+ tb.sense_data_fixed.AdditionalSenseCode = cur_sense_data.asc;
 395+ tb.sense_data_fixed.AdditionalSenseCodeQualifier = cur_sense_data.ascq;
 396+ tb.sense_data_fixed.FieldReplaceableUnitCode = 0;
 397+ tb.sense_data_fixed.SKSV = 0;
 398+ tb.sense_data_fixed.SenseKeySpecific = 0;
 399+ send_command_result(&tb.sense_data_fixed, MIN(sizeof(tb.sense_data_fixed), length));
 400+ }
307401 break;
308402 }
309403
310404 case SCSI_MODE_SENSE_10:
311405 {
312 - if (ums_ejected)
 406+ if (ums_ejected)
313407 {
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;
 408+ send_command_failed_result(SENSE_NOT_READY, ASC_MEDIUM_NOT_PRESENT, 0);
318409 break;
319410 }
320411 unsigned char page_code = cbw->command_block[2] & 0x3f;
——@@ -342,10 +433,7 @@
343434 send_command_result(&tb.ms_data_10, MIN(sizeof(tb.ms_data_10), length));
344435 break;
345436 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;
 437+ send_command_failed_result(SENSE_ILLEGAL_REQUEST, ASC_INVALID_FIELD_IN_CBD, 0);
350438 break;
351439 }
352440 break;
——@@ -355,10 +443,7 @@
356444 {
357445 if (ums_ejected)
358446 {
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;
 447+ send_command_failed_result(SENSE_NOT_READY, ASC_MEDIUM_NOT_PRESENT, 0);
363448 break;
364449 }
365450 unsigned char page_code = cbw->command_block[2] & 0x3f;
——@@ -389,10 +474,7 @@
390475 send_command_result(&tb.ms_data_6, MIN(sizeof(tb.ms_data_6), length));
391476 break;
392477 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;
 478+ send_command_failed_result(SENSE_ILLEGAL_REQUEST, ASC_INVALID_FIELD_IN_CBD, 0);
397479 break;
398480 }
399481 break;
——@@ -419,13 +501,7 @@
420502 tb.format_capacity_data.block_size |= swap32(SCSI_FORMAT_CAPACITY_FORMATTED_MEDIA);
421503 send_command_result(&tb.format_capacity_data, MIN(sizeof(tb.format_capacity_data), length));
422504 }
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 - }
 505+ else send_command_failed_result(SENSE_NOT_READY, ASC_MEDIUM_NOT_PRESENT, 0);
430506 break;
431507 }
432508
——@@ -437,13 +513,7 @@
438514 tb.capacity_data.block_size = swap32(storage_info.sector_size);
439515 send_command_result(&tb.capacity_data, MIN(sizeof(tb.capacity_data), length));
440516 }
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 - }
 517+ else send_command_failed_result(SENSE_NOT_READY, ASC_MEDIUM_NOT_PRESENT, 0);
448518 break;
449519 }
450520
——@@ -450,10 +520,7 @@
451521 case SCSI_READ_10:
452522 if (ums_ejected)
453523 {
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;
 524+ send_command_failed_result(SENSE_NOT_READY, ASC_MEDIUM_NOT_PRESENT, 0);
458525 break;
459526 }
460527 cur_cmd.sector = (cbw->command_block[2] << 24 | cbw->command_block[3] << 16
——@@ -462,12 +529,7 @@
463530 cur_cmd.orig_count = cur_cmd.count;
464531
465532 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 - }
 533+ send_command_failed_result(SENSE_ILLEGAL_REQUEST, ASC_LBA_OUT_OF_RANGE, 0);
472534 else if (!cur_cmd.count) send_csw(0);
473535 else send_and_read_next();
474536 break;
——@@ -475,10 +537,7 @@
476538 case SCSI_WRITE_10:
477539 if (ums_ejected)
478540 {
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;
 541+ send_command_failed_result(SENSE_NOT_READY, ASC_MEDIUM_NOT_PRESENT, 0);
483542 break;
484543 }
485544 cur_cmd.sector = (cbw->command_block[2] << 24 | cbw->command_block[3] << 16
——@@ -487,12 +546,7 @@
488547 cur_cmd.orig_count = cur_cmd.count;
489548
490549 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 - }
 550+ send_command_failed_result(SENSE_ILLEGAL_REQUEST, ASC_LBA_OUT_OF_RANGE, 0);
497551 else if (!cur_cmd.count) send_csw(0);
498552 else
499553 receive_block_data(umsbuf[1][!writebuf_current],
——@@ -501,14 +555,138 @@
502556
503557 case SCSI_WRITE_BUFFER:
504558 break;
505 -
 559+
 560+ case SCSI_ATA_PASSTHROUGH_12:
 561+ case SCSI_ATA_PASSTHROUGH_16:
 562+ {
 563+ if (!storage_info.driverinfo || get_platform_id() != 0x4c435049)
 564+ {
 565+ send_command_failed_result(SENSE_ILLEGAL_REQUEST, ASC_INVALID_COMMAND, 0);
 566+ break;
 567+ }
 568+ int cmd = cbw->command_block[0];
 569+ int multi = cbw->command_block[1] >> 5;
 570+ int protocol = (cbw->command_block[1] >> 1) & 0xf;
 571+ int extend = 0;
 572+ int offline = cbw->command_block[2] >> 6;
 573+ int check = (cbw->command_block[2] >> 5) & 1;
 574+ int type = (cbw->command_block[2] >> 4) & 1;
 575+ int dir = (cbw->command_block[2] >> 3) & 1;
 576+ int block = (cbw->command_block[2] >> 2) & 1;
 577+ int len = cbw->command_block[2] & 3;
 578+ int features = cbw->command_block[3];
 579+ int count = cbw->command_block[4];
 580+ int lbal = cbw->command_block[5];
 581+ int lbam = cbw->command_block[6];
 582+ int lbah = cbw->command_block[7];
 583+ int device = cbw->command_block[8] & ~0x10;
 584+ int command = cbw->command_block[9];
 585+ int control = cbw->command_block[11];
 586+ if (cmd == SCSI_ATA_PASSTHROUGH_16)
 587+ {
 588+ extend = cbw->command_block[1] & 1;
 589+ features = (extend ? (cbw->command_block[3] << 8) : 0) | cbw->command_block[4];
 590+ count = (extend ? (cbw->command_block[5] << 8) : 0) | cbw->command_block[6];
 591+ lbal = (extend ? (cbw->command_block[7] << 8) : 0) | cbw->command_block[8];
 592+ lbam = (extend ? (cbw->command_block[9] << 8) : 0) | cbw->command_block[10];
 593+ lbah = (extend ? (cbw->command_block[11] << 8) : 0) | cbw->command_block[12];
 594+ device = cbw->command_block[13] & ~0x10;
 595+ command = cbw->command_block[14];
 596+ control = cbw->command_block[15];
 597+ }
 598+ sat_command.lba48 = extend;
 599+ sat_check = check;
 600+ int delay = (2 << offline) - 2;
 601+ int bytes = 0;
 602+ switch (len)
 603+ {
 604+ case 1: bytes = features; break;
 605+ case 2: bytes = count; break;
 606+ case 3: bytes = length; break;
 607+ }
 608+ int blocks = 1;
 609+ if (block)
 610+ {
 611+ blocks = bytes;
 612+ bytes = type ? storage_info.sector_size : 512;
 613+ }
 614+ int invalid = blocks * bytes > UMS_BUFSIZE;
 615+ int dma = 0;
 616+ switch (protocol)
 617+ {
 618+ case SAT_HARD_RESET:
 619+ sat_pending = SAT_PENDING_HRST;
 620+ enqueue_async();
 621+ return;
 622+ case SAT_SOFT_RESET:
 623+ sat_pending = SAT_PENDING_SRST;
 624+ enqueue_async();
 625+ return;
 626+ case SAT_NON_DATA:
 627+ if (len) invalid = 1;
 628+ break;
 629+ case SAT_PIO_DATA_IN:
 630+ if (!len || !dir) invalid = 1;
 631+ break;
 632+ case SAT_PIO_DATA_OUT:
 633+ if (!len || dir) invalid = 1;
 634+ break;
 635+ case SAT_UDMA_DATA_IN:
 636+ if (!len || !dir) invalid = 1;
 637+ dma = 1;
 638+ break;
 639+ case SAT_UDMA_DATA_OUT:
 640+ if (!len || dir) invalid = 1;
 641+ dma = 1;
 642+ break;
 643+ case SAT_RETURN_RESPONSE:
 644+ sat_pending = SAT_PENDING_READ_CDB;
 645+ enqueue_async();
 646+ return;
 647+ //case SAT_NON_DATA_RESET:
 648+ //case SAT_DMA:
 649+ //case SAT_DMA_QUEUED:
 650+ //case SAT_FPDMA:
 651+ //case SAT_DIAGNOSTIC:
 652+ default:
 653+ invalid = 1;
 654+ break;
 655+ }
 656+ if (invalid)
 657+ {
 658+ send_command_failed_result(SENSE_ILLEGAL_REQUEST, ASC_INVALID_FIELD_IN_CBD, 0);
 659+ return;
 660+ }
 661+ readbuf_count[!readbuf_current] = 0;
 662+ void* buffer = umsbuf[0][!readbuf_current];
 663+ sat_command.transfer = !!len;
 664+ sat_command.send = !dir;
 665+ sat_command.dma = dma;
 666+ sat_command.delay = delay * 1000000;
 667+ sat_command.buffer = buffer;
 668+ sat_command.size = blocks;
 669+ sat_command.blksize = bytes;
 670+ sat_command.feature = features;
 671+ sat_command.count = count;
 672+ sat_command.lba_low = lbal;
 673+ sat_command.lba_mid = lbam;
 674+ sat_command.lba_high = lbah;
 675+ sat_command.device = device;
 676+ sat_command.command = command;
 677+ if (len && !dir) receive_sat_write(buffer, bytes * blocks);
 678+ else
 679+ {
 680+ sat_pending = SAT_PENDING_CMD;
 681+ enqueue_async();
 682+ }
 683+ break;
 684+ }
 685+
506686 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;
 687+ send_command_failed_result(SENSE_ILLEGAL_REQUEST, ASC_INVALID_COMMAND, 0);
511688 break;
512689 }
 690+ have_sat_response_information = false;
513691 }
514692
515693
——@@ -554,10 +732,8 @@
555733 {
556734 if (IS_ERR(write_rc))
557735 {
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;
 736+ send_command_failed_result(SENSE_MEDIUM_ERROR, ASC_WRITE_ERROR, 0);
 737+ return;
562738 }
563739 send_csw(0);
564740 write_rc = 0;
——@@ -569,34 +745,46 @@
570746 void ums_xfer_complete(bool in, int bytesleft)
571747 {
572748 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 - }
 749+ if (in)
 750+ switch (state)
 751+ {
 752+ case WAITING_FOR_CSW_COMPLETION:
 753+ state = WAITING_FOR_COMMAND;
 754+ listen();
 755+ break;
 756+ case SENDING_RESULT:
 757+ send_csw(0);
 758+ break;
 759+ case SENDING_BLOCKS:
 760+ if (!cur_cmd.count) send_csw(0);
 761+ else send_and_read_next();
 762+ break;
 763+ default:
 764+ fail("diskmode: Got IN completion in invalid state!");
 765+ }
 766+ else
 767+ switch (state)
 768+ {
 769+ case RECEIVING_BLOCKS:
 770+ if (length != storage_info.sector_size * cur_cmd.count && length != UMS_BUFSIZE) break;
 771+ update_readcache();
 772+ if (!writebuf_busy) writebuf_push();
 773+ else writebuf_overrun = true;
 774+ enqueue_async();
 775+ break;
 776+ case WAITING_FOR_COMMAND:
 777+ state = PROCESSING;
 778+ invalidate_dcache(&cmdbuf, sizeof(cmdbuf)); // Who pulls this into a cache line!?
 779+ handle_scsi(&cmdbuf.cbw);
 780+ break;
 781+ case RECEIVING_SAT_WRITE:
 782+ state = PROCESSING;
 783+ sat_pending = SAT_PENDING_CMD;
 784+ enqueue_async();
 785+ break;
 786+ default:
 787+ fail("diskmode: Got OUT completion in invalid state!");
 788+ }
601789 }
602790
603791 void ums_handle_async()
——@@ -625,13 +813,103 @@
626814 {
627815 read_blocked = false;
628816 if (readbuf_count[readbuf_current] < 0)
 817+ send_command_failed_result(SENSE_MEDIUM_ERROR, ASC_READ_ERROR, 0);
 818+ else send_and_read_next();
 819+ }
 820+
 821+ struct ata_target_driverinfo* drv = storage_info.driverinfo;
 822+ switch (sat_pending)
 823+ {
 824+ case SAT_PENDING_SRST:
629825 {
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;
 826+ sat_pending = SAT_PENDING_NONE;
 827+ if (IS_ERR(drv->soft_reset()))
 828+ send_command_failed_result(SENSE_HARDWARE_ERROR, ASC_UNSUCCESSFUL_SOFT_RESET, 0);
 829+ else send_csw(0);
 830+ break;
634831 }
635 - else send_and_read_next();
 832+ case SAT_PENDING_HRST:
 833+ {
 834+ sat_pending = SAT_PENDING_NONE;
 835+ if (IS_ERR(drv->hard_reset()))
 836+ send_command_failed_result(SENSE_HARDWARE_ERROR, ASC_UNSUCCESSFUL_SOFT_RESET, 0);
 837+ else send_csw(0);
 838+ break;
 839+ }
 840+ case SAT_PENDING_CMD:
 841+ {
 842+int cmd = sat_command.command;
 843+ int datasize = sat_command.size * sat_command.blksize;
 844+ void* buffer = sat_command.buffer;
 845+ int rc = drv->raw_cmd(&sat_command);
 846+ sat_response_information.descriptor_code = 9;
 847+ sat_response_information.additional_length = 12;
 848+ sat_response_information.extend = sat_command.lba48;
 849+ sat_response_information.error = sat_command.feature;
 850+ sat_response_information.sector_count_h = sat_command.count >> 8;
 851+ sat_response_information.sector_count_l = sat_command.count & 0xff;
 852+ sat_response_information.lba_low_h = sat_command.lba_low >> 8;
 853+ sat_response_information.lba_low_l = sat_command.lba_low & 0xff;
 854+ sat_response_information.lba_mid_h = sat_command.lba_mid >> 8;
 855+ sat_response_information.lba_mid_l = sat_command.lba_mid & 0xff;
 856+ sat_response_information.lba_high_h = sat_command.lba_high >> 8;
 857+ sat_response_information.lba_high_l = sat_command.lba_high & 0xff;
 858+ sat_response_information.device = sat_command.device;
 859+ sat_response_information.status = sat_command.command;
 860+ have_sat_response_information = true;
 861+ sat_pending = SAT_PENDING_NONE;
 862+ if (IS_ERR(rc) || (sat_command.command & 0x21))
 863+ {
 864+ if (sat_command.command & 0x20)
 865+ send_command_failed_result(SENSE_HARDWARE_ERROR, ASC_INTERNAL_TARGET_FAILURE, 0);
 866+ else if ((sat_command.command & 0x01) && (sat_command.feature & 0x01))
 867+ send_command_failed_result(SENSE_MEDIUM_ERROR, ASC_ADDRESS_MARK_NOT_FOUND, 0);
 868+ else if ((sat_command.command & 0x01) && (sat_command.feature & 0x02))
 869+ send_command_failed_result(SENSE_NOT_READY, ASC_MEDIUM_NOT_PRESENT, 0);
 870+ else if ((sat_command.command & 0x01) && (sat_command.feature & 0x08))
 871+ send_command_failed_result(SENSE_UNIT_ATTENTION, ASC_OPERATOR_REQUEST, ASCQ_MEDIUM_REMOVAL_REQUEST);
 872+ else if ((sat_command.command & 0x01) && (sat_command.feature & 0x10))
 873+ send_command_failed_result(SENSE_ILLEGAL_REQUEST, ASC_LBA_OUT_OF_RANGE, 0);
 874+ else if ((sat_command.command & 0x01) && (sat_command.feature & 0x20))
 875+ send_command_failed_result(SENSE_UNIT_ATTENTION, ASC_MEDIUM_MAY_HAVE_CHANGED, 0);
 876+ else if ((sat_command.command & 0x01) && (sat_command.feature & 0x40) && datasize && !sat_command.send)
 877+ send_command_failed_result(SENSE_MEDIUM_ERROR, ASC_READ_ERROR, 0);
 878+ else if ((sat_command.command & 0x01) && (sat_command.feature & 0x40))
 879+ send_command_failed_result(SENSE_DATA_PROTECT, ASC_WRITE_PROTECTED, 0);
 880+ else if ((sat_command.command & 0x01) && (sat_command.feature & 0x80))
 881+ send_command_failed_result(SENSE_ABORTED_COMMAND, ASC_SCSI_PARITY_ERROR, ASCQ_UICRC_ERROR_DETECTED);
 882+ else if ((sat_command.command & 0x01) && (sat_command.feature & 0x04))
 883+ send_command_failed_result(SENSE_ABORTED_COMMAND, 0, 0);
 884+ else send_command_failed_result(SENSE_HARDWARE_ERROR, ASC_INTERNAL_TARGET_FAILURE, 0);
 885+ }
 886+ else if (sat_check)
 887+ send_command_failed_result(SENSE_SOFT_ERROR, ASC_RECOVERED_ERROR, ASCQ_SAT_INFO_AVAILABLE);
 888+ else if (datasize) send_command_result(buffer, datasize);
 889+ else send_csw(0);
 890+ break;
 891+ }
 892+ case SAT_PENDING_READ_CDB:
 893+ {
 894+ int rc = drv->read_taskfile(&sat_command);
 895+ sat_response_information.descriptor_code = 9;
 896+ sat_response_information.additional_length = 12;
 897+ sat_response_information.extend = sat_command.lba48;
 898+ sat_response_information.error = sat_command.feature;
 899+ sat_response_information.sector_count_h = sat_command.count >> 8;
 900+ sat_response_information.sector_count_l = sat_command.count & 0xff;
 901+ sat_response_information.lba_low_h = sat_command.lba_low >> 8;
 902+ sat_response_information.lba_low_l = sat_command.lba_low & 0xff;
 903+ sat_response_information.lba_mid_h = sat_command.lba_mid >> 8;
 904+ sat_response_information.lba_mid_l = sat_command.lba_mid & 0xff;
 905+ sat_response_information.lba_high_h = sat_command.lba_high >> 8;
 906+ sat_response_information.lba_high_l = sat_command.lba_high & 0xff;
 907+ sat_response_information.device = sat_command.device;
 908+ sat_response_information.status = sat_command.command;
 909+ sat_pending = SAT_PENDING_NONE;
 910+ if (IS_ERR(rc)) send_command_failed_result(SENSE_HARDWARE_ERROR, ASC_INTERNAL_TARGET_FAILURE, 0);
 911+ else send_command_result(&sat_response_information, sizeof(sat_response_information));
 912+ break;
 913+ }
636914 }
637915 }
638916
Index: apps/diskmode/scsi.h
——@@ -41,11 +41,17 @@
4242 #define SCSI_START_STOP_UNIT 0x1b
4343 #define SCSI_REPORT_LUNS 0xa0
4444 #define SCSI_WRITE_BUFFER 0x3b
 45+#define SCSI_ATA_PASSTHROUGH_12 0xa1
 46+#define SCSI_ATA_PASSTHROUGH_16 0x85
4547
 48+#define SENSE_SOFT_ERROR 0x01
4649 #define SENSE_NOT_READY 0x02
4750 #define SENSE_MEDIUM_ERROR 0x03
 51+#define SENSE_HARDWARE_ERROR 0x04
4852 #define SENSE_ILLEGAL_REQUEST 0x05
4953 #define SENSE_UNIT_ATTENTION 0x06
 54+#define SENSE_DATA_PROTECT 0x07
 55+#define SENSE_ABORTED_COMMAND 0x0b
5056
5157 #define ASC_MEDIUM_NOT_PRESENT 0x3a
5258 #define ASC_INVALID_FIELD_IN_CBD 0x24
——@@ -54,9 +60,34 @@
5561 #define ASC_READ_ERROR 0x11
5662 #define ASC_NOT_READY 0x04
5763 #define ASC_INVALID_COMMAND 0x20
 64+#define ASC_INTERNAL_TARGET_FAILURE 0x44
 65+#define ASC_ADDRESS_MARK_NOT_FOUND 0x13
 66+#define ASC_OPERATOR_REQUEST 0x5a
 67+#define ASC_MEDIUM_MAY_HAVE_CHANGED 0x28
 68+#define ASC_SCSI_PARITY_ERROR 0x47
 69+#define ASC_WRITE_PROTECTED 0x27
 70+#define ASC_RECOVERED_ERROR 0x00
 71+#define ASC_UNSUCCESSFUL_SOFT_RESET 0x46
5872
5973 #define ASCQ_BECOMING_READY 0x01
 74+#define ASCQ_MEDIUM_REMOVAL_REQUEST 0x01
 75+#define ASCQ_UICRC_ERROR_DETECTED 0x03
 76+#define ASCQ_SAT_INFO_AVAILABLE 0x1d
6077
 78+#define SAT_HARD_RESET 0x0
 79+#define SAT_SOFT_RESET 0x1
 80+#define SAT_NON_DATA 0x3
 81+#define SAT_PIO_DATA_IN 0x4
 82+#define SAT_PIO_DATA_OUT 0x5
 83+#define SAT_DMA 0x6
 84+#define SAT_DMA_QUEUED 0x7
 85+#define SAT_DIAGNOSTIC 0x8
 86+#define SAT_NON_DATA_RESET 0x9
 87+#define SAT_UDMA_DATA_IN 0xa
 88+#define SAT_UDMA_DATA_OUT 0xb
 89+#define SAT_FPDMA 0xc
 90+#define SAT_RETURN_RESPONSE 0xf
 91+
6192 #define DIRECT_ACCESS_DEVICE 0x00
6293 #define DEVICE_REMOVABLE 0x80
6394
——@@ -84,7 +115,7 @@
85116 unsigned char luns[1][8];
86117 };
87118
88 -struct __attribute__((packed)) sense_data
 119+struct __attribute__((packed)) sense_data_fixed
89120 {
90121 unsigned char ResponseCode;
91122 unsigned char Obsolete;
——@@ -99,6 +130,16 @@
100131 unsigned short SenseKeySpecific;
101132 };
102133
 134+struct __attribute__((packed)) sense_data_descr
 135+{
 136+ unsigned char ResponseCode;
 137+ unsigned char fei_sensekey;
 138+ unsigned char AdditionalSenseCode;
 139+ unsigned char AdditionalSenseCodeQualifier;
 140+ unsigned char Reserved[3];
 141+ unsigned char AdditionalSenseLength;
 142+};
 143+
103144 struct __attribute__((packed)) mode_sense_bdesc_longlba
104145 {
105146 unsigned char num_blocks[8];
Retrieved from "https://freemyipod.org/wiki/Special:Code/freemyipod/967"