1
/*
2
 * USB Gadget UMS driver for Tegra X1
3
 *
4
 * Copyright (c) 2003-2008 Alan Stern
5
 * Copyright (c) 2009 Samsung Electronics
6
 *                    Author: Michal Nazarewicz <[email protected]>
7
 * Copyright (c) 2019-2024 CTCaer
8
 *
9
 * This program is free software; you can redistribute it and/or modify it
10
 * under the terms and conditions of the GNU General Public License,
11
 * version 2, as published by the Free Software Foundation.
12
 *
13
 * This program is distributed in the hope it will be useful, but WITHOUT
14
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
15
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
16
 * more details.
17
 *
18
 * You should have received a copy of the GNU General Public License
19
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
20
 */
21

22
#include <string.h>
23

24
#include <usb/usbd.h>
25
#include <gfx_utils.h>
26
#include <soc/hw_init.h>
27
#include <soc/timer.h>
28
#include <soc/t210.h>
29
#include <storage/sd.h>
30
#include <storage/sdmmc.h>
31
#include <storage/sdmmc_driver.h>
32
#include <utils/btn.h>
33
#include <utils/sprintf.h>
34

35
#include <memory_map.h>
36

37
//#define DPRINTF(...) gfx_printf(__VA_ARGS__)
38
#define DPRINTF(...)
39

40
#define UMS_MAX_LUN 1 // Only 1 disk/partition for now.
41

42
#define USB_BULK_CB_WRAP_LEN 31
43
#define USB_BULK_CB_SIG      0x43425355 // USBC.
44
#define USB_BULK_IN_FLAG     0x80
45

46
#define USB_BULK_CS_WRAP_LEN 13
47
#define USB_BULK_CS_SIG      0x53425355 // USBS.
48

49
#define USB_STATUS_PASS        0
50
#define USB_STATUS_FAIL        1
51
#define USB_STATUS_PHASE_ERROR 2
52

53
#define UMS_DISK_LBA_SHIFT 9
54
#define UMS_DISK_LBA_SIZE  (1 << UMS_DISK_LBA_SHIFT)
55

56
#define UMS_DISK_MAX_IO_TRANSFER_64K (USB_EP_BUFFER_MAX_SIZE >> UMS_DISK_LBA_SHIFT)
57
#define UMS_DISK_MAX_IO_TRANSFER_32K (UMS_DISK_MAX_IO_TRANSFER_64K / 2)
58

59
#define UMS_SCSI_TRANSFER_512K (0x80000 >> UMS_DISK_LBA_SHIFT)
60

61
#define UMS_EP_OUT_MAX_XFER (USB_EP_BULK_OUT_MAX_XFER)
62

63
// Length of a SCSI Command Data Block.
64
#define SCSI_MAX_CMD_SZ 16
65

66
// SCSI device types
67
#define SCSI_TYPE_DISK  0x00
68

69
// SCSI commands.
70
#define SC_FORMAT_UNIT        0x04
71
#define SC_INQUIRY            0x12
72
#define SC_LOG_SENSE          0x4D
73
#define SC_MODE_SELECT_6      0x15
74
#define SC_MODE_SELECT_10     0x55
75
#define SC_MODE_SENSE_6       0x1A
76
#define SC_MODE_SENSE_10      0x5A
77
#define SC_PREVENT_ALLOW_MEDIUM_REMOVAL 0x1E
78
#define SC_READ_6             0x08
79
#define SC_READ_10            0x28
80
#define SC_READ_12            0xA8
81
#define SC_READ_CAPACITY      0x25
82
#define SC_READ_FORMAT_CAPACITIES 0x23
83
#define SC_READ_HEADER        0x44
84
#define SC_READ_TOC           0x43
85
#define SC_RELEASE            0x17
86
#define SC_REQUEST_SENSE      0x03
87
#define SC_RESERVE            0x16
88
#define SC_SEND_DIAGNOSTIC    0x1D
89
#define SC_START_STOP_UNIT    0x1B
90
#define SC_SYNCHRONIZE_CACHE  0x35
91
#define SC_TEST_UNIT_READY    0x00
92
#define SC_VERIFY             0x2F
93
#define SC_WRITE_6            0x0A
94
#define SC_WRITE_10           0x2A
95
#define SC_WRITE_12           0xAA
96

97
// SCSI Sense Key/Additional Sense Code/ASC Qualifier values.
98
#define SS_NO_SENSE                           0x0
99
#define SS_COMMUNICATION_FAILURE              0x40800
100
#define SS_INVALID_COMMAND                    0x52000
101
#define SS_INVALID_FIELD_IN_CDB               0x52400
102
#define SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE 0x52100
103
#define SS_MEDIUM_NOT_PRESENT                 0x23A00
104
#define SS_MEDIUM_REMOVAL_PREVENTED           0x55302
105
#define SS_NOT_READY_TO_READY_TRANSITION      0x62800
106
#define SS_RESET_OCCURRED                     0x62900
107
#define SS_SAVING_PARAMETERS_NOT_SUPPORTED    0x53900
108
#define SS_UNRECOVERED_READ_ERROR             0x31100
109
#define SS_WRITE_ERROR                        0x30C02
110
#define SS_WRITE_PROTECTED                    0x72700
111

112
#define SK(x)   ((u8) ((x) >> 16)) // Sense Key byte, etc.
113
#define ASC(x)  ((u8) ((x) >> 8))
114
#define ASCQ(x) ((u8) (x))
115

116
enum ums_state {
117
	UMS_STATE_NORMAL = 0,
118
	UMS_STATE_ABORT_BULK_OUT,
119
	UMS_STATE_PROTOCOL_RESET,
120
	UMS_STATE_EXIT,
121
	UMS_STATE_TERMINATED
122
};
123

124
enum ums_result {
125
	UMS_RES_OK          = 0,
126
	UMS_RES_IO_ERROR    = -5,
127
	UMS_RES_TIMEOUT     = -3,
128
	UMS_RES_PROT_FATAL  = -4,
129
	UMS_RES_INVALID_ARG = -22
130
};
131

132

133
enum data_direction {
134
	DATA_DIR_UNKNOWN = 0,
135
	DATA_DIR_FROM_HOST,
136
	DATA_DIR_TO_HOST,
137
	DATA_DIR_NONE
138
};
139

140
enum buffer_state {
141
	BUF_STATE_EMPTY = 0,
142
	BUF_STATE_FULL,
143
	BUF_STATE_BUSY
144
};
145

146
typedef struct _bulk_recv_pkt_t {
147
	u32 Signature;          // 'USBC'.
148
	u32 Tag;                // Unique per command id.
149
	u32 DataTransferLength; // Size of the data.
150
	u8  Flags;              // Direction in bit 7.
151
	u8  Lun;                // LUN (normally 0).
152
	u8  Length;             // Of the CDB, <= SCSI_MAX_CMD_SZ.
153
	u8  CDB[16];            // Command Data Block.
154
} bulk_recv_pkt_t;
155

156
typedef struct _bulk_send_pkt_t {
157
	u32 Signature; // 'USBS'.
158
	u32 Tag;       // Same as original command.
159
	u32 Residue;   // Amount not transferred.
160
	u8  Status;
161
} bulk_send_pkt_t;
162

163
typedef struct _logical_unit_t
164
{
165
	sdmmc_t *sdmmc;
166
	sdmmc_storage_t *storage;
167

168
	u32 num_sectors;
169
	u32 offset;
170

171
	int unmounted;
172

173
	u32 ro;
174
	u32 type;
175
	u32 partition;
176
	u32 removable;
177
	u32 prevent_medium_removal;
178

179
	u32 info_valid;
180

181
	u32 sense_data;
182
	u32 sense_data_info;
183
	u32 unit_attention_data;
184
} logical_unit_t;
185

186
typedef struct _bulk_ctxt_t {
187
	u32  bulk_in;
188
	int  bulk_in_status;
189
	u32  bulk_in_length;
190
	u32  bulk_in_length_actual;
191
	u8  *bulk_in_buf;
192
	enum buffer_state bulk_in_buf_state;
193

194
	u32  bulk_out;
195
	int  bulk_out_status;
196
	u32  bulk_out_length;
197
	u32  bulk_out_length_actual;
198
	int  bulk_out_ignore;
199
	u8  *bulk_out_buf;
200
	enum buffer_state bulk_out_buf_state;
201
} bulk_ctxt_t;
202

203
typedef struct _usbd_gadget_ums_t {
204
	bulk_ctxt_t bulk_ctxt;
205

206
	u32  cmnd_size;
207
	u8   cmnd[SCSI_MAX_CMD_SZ];
208

209
	u32  lun_idx; // lun index
210
	logical_unit_t lun;
211

212
	enum ums_state state; // For exception handling.
213

214
	enum data_direction data_dir;
215
	u32  data_size;
216
	u32  data_size_from_cmnd;
217
	u32  tag;
218
	u32  residue;
219
	u32  usb_amount_left;
220
	bool cbw_req_queued;
221

222
	u32  phase_error;
223
	u32  short_packet_received;
224

225
	int  thread_wakeup_needed;
226
	int  can_stall;
227

228
	u32 timeouts;
229
	bool xusb;
230

231
	void (*system_maintenance)(bool);
232
	void *label;
233
	void (*set_text)(void *, const char *);
234
} usbd_gadget_ums_t;
235

236
static usb_ops_t usb_ops;
237

238
static inline void put_array_le_to_be16(u16 val, void *p)
239
{
240
	u8 *_p = p;
241
	_p[0] = val >> 8;
242
	_p[1] = val;
243
}
244

245
static inline void put_array_le_to_be32(u32 val, void *p)
246
{
247
	u8 *_p = p;
248
	_p[0] = val >> 24;
249
	_p[1] = val >> 16;
250
	_p[2] = val >> 8;
251
	_p[3] = val;
252
}
253

254
static inline u16 get_array_be_to_le16(const void *p)
255
{
256
	const u8 *_p = p;
257
	u16 val = _p[0] << 8 | _p[1];
258
	return val;
259
}
260

261
static inline u32 get_array_be_to_le24(const void *p)
262
{
263
	const u8 *_p = p;
264
	u32 val = (_p[0] << 16) | (_p[1] << 8) | _p[2];
265
	return val;
266
}
267

268
static inline u32 get_array_be_to_le32(const void *p)
269
{
270
	const u8 *_p = p;
271
	u32 val = (_p[0] << 24) | (_p[1] << 16) | (_p[2] << 8) | _p[3];
272
	return val;
273
}
274

275
static void raise_exception(usbd_gadget_ums_t *ums, enum ums_state new_state)
276
{
277
	/* Do nothing if a higher-priority exception is already in progress.
278
	 * If a lower-or-equal priority exception is in progress, preempt it
279
	 * and notify the main thread by sending it a signal. */
280
	if (ums->state <= new_state) {
281
		ums->state = new_state;
282
		ums->thread_wakeup_needed = 1;
283
	}
284
}
285

286
static void _handle_ep0_ctrl(usbd_gadget_ums_t *ums)
287
{
288
	if (usb_ops.usbd_handle_ep0_ctrl_setup())
289
		raise_exception(ums, UMS_STATE_PROTOCOL_RESET);
290
}
291

292
static int _wedge_bulk_in_endpoint(usbd_gadget_ums_t *ums)
293
{
294
	/* usbd_set_ep_wedge(bulk_ctxt->bulk_in); */
295

296
	return UMS_RES_OK;
297
}
298

299
static int _set_ep_stall(u32 ep)
300
{
301
	usb_ops.usbd_set_ep_stall(ep, USB_EP_CFG_STALL);
302

303
	return UMS_RES_OK;
304
}
305

306
static int _clear_ep_stall(u32 ep)
307
{
308
	usb_ops.usbd_set_ep_stall(ep, USB_EP_CFG_CLEAR);
309

310
	return UMS_RES_OK;
311
}
312

313
static void _flush_endpoint(u32 ep)
314
{
315
	if (usb_ops.usbd_flush_endpoint)
316
		usb_ops.usbd_flush_endpoint(ep);
317
}
318

319
static void _transfer_start(usbd_gadget_ums_t *ums, bulk_ctxt_t *bulk_ctxt, u32 ep, u32 sync_timeout)
320
{
321
	if (ep == bulk_ctxt->bulk_in)
322
	{
323
		bulk_ctxt->bulk_in_status = usb_ops.usb_device_ep1_in_write(
324
			bulk_ctxt->bulk_in_buf, bulk_ctxt->bulk_in_length,
325
			&bulk_ctxt->bulk_in_length_actual, sync_timeout);
326

327
		if (bulk_ctxt->bulk_in_status == USB_ERROR_XFER_ERROR)
328
		{
329
			ums->set_text(ums->label, "#FFDD00 Error:# EP IN transfer!");
330
			_flush_endpoint(bulk_ctxt->bulk_in);
331
		}
332
		else if (bulk_ctxt->bulk_in_status == USB2_ERROR_XFER_NOT_ALIGNED)
333
			ums->set_text(ums->label, "#FFDD00 Error:# EP IN Buffer not aligned!");
334

335
		if (sync_timeout)
336
			bulk_ctxt->bulk_in_buf_state = BUF_STATE_EMPTY;
337
	}
338
	else
339
	{
340
		bulk_ctxt->bulk_out_status = usb_ops.usb_device_ep1_out_read(
341
			bulk_ctxt->bulk_out_buf, bulk_ctxt->bulk_out_length,
342
			&bulk_ctxt->bulk_out_length_actual, sync_timeout);
343

344
		if (bulk_ctxt->bulk_out_status == USB_ERROR_XFER_ERROR)
345
		{
346
			ums->set_text(ums->label, "#FFDD00 Error:# EP OUT transfer!");
347
			_flush_endpoint(bulk_ctxt->bulk_out);
348
		}
349
		else if (bulk_ctxt->bulk_out_status == USB2_ERROR_XFER_NOT_ALIGNED)
350
			ums->set_text(ums->label, "#FFDD00 Error:# EP OUT Buffer not aligned!");
351

352
		if (sync_timeout)
353
			bulk_ctxt->bulk_out_buf_state = BUF_STATE_FULL;
354
	}
355
}
356

357
static void _transfer_out_big_read(usbd_gadget_ums_t *ums, bulk_ctxt_t *bulk_ctxt)
358
{
359
		bulk_ctxt->bulk_out_status = usb_ops.usb_device_ep1_out_read_big(
360
			bulk_ctxt->bulk_out_buf, bulk_ctxt->bulk_out_length,
361
			&bulk_ctxt->bulk_out_length_actual);
362

363
		if (bulk_ctxt->bulk_out_status == USB_ERROR_XFER_ERROR)
364
		{
365
			ums->set_text(ums->label, "#FFDD00 Error:# EP OUT transfer!");
366
			_flush_endpoint(bulk_ctxt->bulk_out);
367
		}
368

369
		bulk_ctxt->bulk_out_buf_state = BUF_STATE_FULL;
370
}
371

372
static void _transfer_finish(usbd_gadget_ums_t *ums, bulk_ctxt_t *bulk_ctxt, u32 ep, u32 sync_timeout)
373
{
374
	if (ep == bulk_ctxt->bulk_in)
375
	{
376
		bulk_ctxt->bulk_in_status = usb_ops.usb_device_ep1_in_writing_finish(
377
			&bulk_ctxt->bulk_in_length_actual, sync_timeout);
378

379
		if (bulk_ctxt->bulk_in_status == USB_ERROR_XFER_ERROR)
380
		{
381
			ums->set_text(ums->label, "#FFDD00 Error:# EP IN transfer!");
382
			_flush_endpoint(bulk_ctxt->bulk_in);
383
		}
384

385
		bulk_ctxt->bulk_in_buf_state = BUF_STATE_EMPTY;
386
	}
387
	else
388
	{
389
		bulk_ctxt->bulk_out_status = usb_ops.usb_device_ep1_out_reading_finish(
390
			&bulk_ctxt->bulk_out_length_actual, sync_timeout);
391

392
		if (bulk_ctxt->bulk_out_status == USB_ERROR_XFER_ERROR)
393
		{
394
			ums->set_text(ums->label, "#FFDD00 Error:# EP OUT transfer!");
395
			_flush_endpoint(bulk_ctxt->bulk_out);
396
		}
397

398
		bulk_ctxt->bulk_out_buf_state = BUF_STATE_FULL;
399
	}
400
}
401

402
static void _reset_buffer(bulk_ctxt_t *bulk_ctxt, u32 ep)
403
{
404
	if (ep == bulk_ctxt->bulk_in)
405
		bulk_ctxt->bulk_in_buf  = (u8 *)USB_EP_BULK_IN_BUF_ADDR;
406
	else
407
		bulk_ctxt->bulk_out_buf = (u8 *)USB_EP_BULK_OUT_BUF_ADDR;
408
}
409

410
/*
411
 * The following are old data based on max 64KB SCSI transfers.
412
 * The endpoint xfer is actually 41.2 MB/s and SD card max 39.2 MB/s, with higher SCSI
413
 * transfers, but the concurrency still helps and increases speeds by 20%.
414
 *
415
 * Concurrency of the SDMMC and USB xfers is very important with no cache.
416
 * The worst offender being the SD card. We are already limited by bus, so
417
 * concurrency helps minimize the SDMMC overhead.
418
 * Max achieved bulk endpoint rate on a Tegra X1 and USB2.0 is 39.4 MB/s.
419
 *
420
 * USB bulk endpoint raw max transfer rate:
421
 * 39.4MB/S - SCSI 128KB.
422
 * 38.2MB/s - SCSI  64KB.
423
 *
424
 *     128 KB,      64 KB,      32 KB,     16 KB,      8 KB - Internal SDMMC I\O Sizes
425
 * -------------------------------------------------------------------------------------
426
 * eMMC - Toshiba - 4MB reads: 314.8 MB/s:
427
 * 225.9 MB/s, 168.6 MB/s, 114.7 MB/s, 86.4 MB/s, 50.3 MB/s - RAW SDMMC.
428
 *  33.5 MB/s,  31.9 MB/s,  29.3 MB/s, 27.1 MB/s, 22.1 MB/s - SCSI 128KB, No concurrency.
429
 *  33.5 MB/s,  35.3 MB/s,  36.3 MB/s, 37.3 MB/s, 37.8 MB/s - SCSI 128KB, Concurrency.
430
 *  --.- --/-,  31.1 MB/s,  28.7 MB/s, 26.5 MB/s, 21.7 MB/s - SCSI  64KB, No concurrency.
431
 *  --.- --/-,  31.1 MB/s,  32.7 MB/s, 34.4 MB/s, 35.0 MB/s - SCSI  64KB, Concurrency.
432
 *
433
 * SD Card - Samsung Evo+ 128GB - 4MB reads: 91.6 MB/s:
434
 *  72.6 MB/s,  62.8 MB/s,  47.4 MB/s, 31.1 MB/s, 18.5 MB/s - RAW SDMMC.
435
 *  25.5 MB/s,  24.2 MB/s,  21.5 MB/s, 17.4 MB/s, 12.6 MB/s - SCSI 128KB, No concurrency.
436
 *  25.5 MB/s,  30.0 MB/s,  32.6 MB/s, 28.3 MB/s, 18.0 MB/s - SCSI 128KB, Concurrency.
437
 *  --.- --/-,  23.8 MB/s,  21.2 MB/s, 17.1 MB/s, 12.5 MB/s - SCSI  64KB, No concurrency.
438
 *  --.- --/-,  23.8 MB/s,  27.2 MB/s, 25.8 MB/s, 17.5 MB/s - SCSI  64KB, Concurrency.
439
 */
440

441
static int _scsi_read(usbd_gadget_ums_t *ums, bulk_ctxt_t *bulk_ctxt)
442
{
443
	u32 lba_offset;
444
	bool first_read = true;
445
	u8 *sdmmc_buf = (u8 *)SDXC_BUF_ALIGNED;
446

447
	// Get the starting LBA and check that it's not too big.
448
	if (ums->cmnd[0] == SC_READ_6)
449
		lba_offset = get_array_be_to_le24(&ums->cmnd[1]);
450
	else
451
	{
452
		lba_offset = get_array_be_to_le32(&ums->cmnd[2]);
453

454
		// We allow DPO and FUA bypass cache bits, but we don't use them.
455
		if ((ums->cmnd[1] & ~0x18) != 0)
456
		{
457
			ums->lun.sense_data = SS_INVALID_FIELD_IN_CDB;
458

459
			return UMS_RES_INVALID_ARG;
460
		}
461
	}
462
	if (lba_offset >= ums->lun.num_sectors)
463
	{
464
		ums->set_text(ums->label, "#FF8000 Warn:# Read - Out of range! Host notified.");
465
		ums->lun.sense_data = SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
466

467
		return UMS_RES_INVALID_ARG;
468
	}
469

470
	// Check that request data size is not 0.
471
	u32 amount_left = ums->data_size_from_cmnd >> UMS_DISK_LBA_SHIFT;
472
	if (!amount_left)
473
		return UMS_RES_IO_ERROR; // No default reply.
474

475
	// Limit IO transfers based on request for faster concurrent reads.
476
	u32 max_io_transfer = (amount_left >= UMS_SCSI_TRANSFER_512K) ?
477
						  UMS_DISK_MAX_IO_TRANSFER_64K : UMS_DISK_MAX_IO_TRANSFER_32K;
478

479
	while (true)
480
	{
481
		// Max io size and end sector limits.
482
		u32 amount = MIN(amount_left, max_io_transfer);
483
		amount     = MIN(amount, ums->lun.num_sectors - lba_offset);
484

485
		// Check if it is a read past the end sector.
486
		if (!amount)
487
		{
488
			ums->lun.sense_data      = SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
489
			ums->lun.sense_data_info = lba_offset;
490
			ums->lun.info_valid      = 1;
491

492
			bulk_ctxt->bulk_in_length = 0;
493
			bulk_ctxt->bulk_in_buf_state = BUF_STATE_FULL;
494
			break;
495
		}
496

497
		// Do the SDMMC read.
498
		if (!sdmmc_storage_read(ums->lun.storage, ums->lun.offset + lba_offset, amount, sdmmc_buf))
499
			amount = 0;
500

501
		// Wait for the async USB transfer to finish.
502
		if (!first_read)
503
			_transfer_finish(ums, bulk_ctxt, bulk_ctxt->bulk_in, USB_XFER_SYNCED);
504

505
		lba_offset   += amount;
506
		amount_left  -= amount;
507
		ums->residue -= amount << UMS_DISK_LBA_SHIFT;
508

509
		bulk_ctxt->bulk_in_length    = amount << UMS_DISK_LBA_SHIFT;
510
		bulk_ctxt->bulk_in_buf_state = BUF_STATE_FULL;
511
		bulk_ctxt->bulk_in_buf       = sdmmc_buf;
512

513
		// If an error occurred, report it and its position.
514
		if (!amount)
515
		{
516
			ums->set_text(ums->label, "#FFDD00 Error:# SDMMC Read!");
517
			ums->lun.sense_data      = SS_UNRECOVERED_READ_ERROR;
518
			ums->lun.sense_data_info = lba_offset;
519
			ums->lun.info_valid      = 1;
520
			break;
521
		}
522

523
		// Last SDMMC read. Last part will be sent by the finish reply function.
524
		if (!amount_left)
525
			break;
526

527
		// Start the USB transfer.
528
		_transfer_start(ums, bulk_ctxt, bulk_ctxt->bulk_in, USB_XFER_START);
529
		first_read = false;
530

531
		// Increment our buffer to read new data.
532
		sdmmc_buf += amount << UMS_DISK_LBA_SHIFT;
533
	}
534

535
	return UMS_RES_IO_ERROR; // No default reply.
536
}
537

538
/*
539
 * Writes are another story.
540
 * Tests showed that big writes are faster than concurrent 32K usb reads + writes.
541
 * The only thing that can help here is caching the writes. But for the simplicity
542
 * of this implementation it will not be implemented yet.
543
 */
544

545
static int _scsi_write(usbd_gadget_ums_t *ums, bulk_ctxt_t *bulk_ctxt)
546
{
547
	static char txt_buf[256];
548
	u32 amount_left_to_req, amount_left_to_write;
549
	u32 usb_lba_offset, lba_offset;
550
	u32 amount;
551

552
	if (ums->lun.ro)
553
	{
554
		ums->set_text(ums->label, "#FF8000 Warn:# Write - Read only! Host notified.");
555
		ums->lun.sense_data = SS_WRITE_PROTECTED;
556

557
		return UMS_RES_INVALID_ARG;
558
	}
559

560
	if (ums->cmnd[0] == SC_WRITE_6)
561
		lba_offset = get_array_be_to_le24(&ums->cmnd[1]);
562
	else
563
	{
564
		lba_offset = get_array_be_to_le32(&ums->cmnd[2]);
565

566
		// We allow DPO and FUA bypass cache bits. We only implement FUA by performing synchronous output.
567
		if (ums->cmnd[1] & ~0x18)
568
		{
569
			ums->lun.sense_data = SS_INVALID_FIELD_IN_CDB;
570

571
			return UMS_RES_INVALID_ARG;
572
		}
573
	}
574

575
	// Check that starting LBA is not past the end sector offset.
576
	if (lba_offset >= ums->lun.num_sectors)
577
	{
578
		ums->set_text(ums->label, "#FF8000 Warn:# Write - Out of range! Host notified.");
579
		ums->lun.sense_data = SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
580

581
		return UMS_RES_INVALID_ARG;
582
	}
583

584
	// Carry out the file writes.
585
	usb_lba_offset       = lba_offset;
586
	amount_left_to_req   = ums->data_size_from_cmnd;
587
	amount_left_to_write = ums->data_size_from_cmnd;
588

589
	while (amount_left_to_write > 0)
590
	{
591
		// Queue a request for more data from the host.
592
		if (amount_left_to_req > 0)
593
		{
594

595
			// Limit write to max supported read from EP OUT.
596
			amount = MIN(amount_left_to_req, UMS_EP_OUT_MAX_XFER);
597

598
			if (usb_lba_offset >= ums->lun.num_sectors)
599
			{
600
				ums->set_text(ums->label, "#FFDD00 Error:# Write - Past last sector!");
601
				ums->lun.sense_data      = SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
602
				ums->lun.sense_data_info = usb_lba_offset;
603
				ums->lun.info_valid      = 1;
604
				break;
605
			}
606

607
			// Get the next buffer.
608
			usb_lba_offset       += amount >> UMS_DISK_LBA_SHIFT;
609
			ums->usb_amount_left -= amount;
610
			amount_left_to_req   -= amount;
611

612
			bulk_ctxt->bulk_out_length = amount;
613

614
			_transfer_out_big_read(ums, bulk_ctxt);
615
		}
616

617
		if (bulk_ctxt->bulk_out_buf_state == BUF_STATE_FULL)
618
		{
619
			bulk_ctxt->bulk_out_buf_state = BUF_STATE_EMPTY;
620

621
			// Did something go wrong with the transfer?.
622
			if (bulk_ctxt->bulk_out_status != 0)
623
			{
624
				ums->lun.sense_data      = SS_COMMUNICATION_FAILURE;
625
				ums->lun.sense_data_info = lba_offset;
626
				ums->lun.info_valid      = 1;
627

628
				s_printf(txt_buf, "#FFDD00 Error:# Write - Comm failure %d!", bulk_ctxt->bulk_out_status);
629
				ums->set_text(ums->label, txt_buf);
630
				break;
631
			}
632

633
			amount = bulk_ctxt->bulk_out_length_actual;
634

635
			if ((ums->lun.num_sectors - lba_offset) < (amount >> UMS_DISK_LBA_SHIFT))
636
			{
637
				DPRINTF("write %X @ %X beyond end %X\n", amount, lba_offset, ums->lun.num_sectors);
638
				amount = (ums->lun.num_sectors - lba_offset) << UMS_DISK_LBA_SHIFT;
639
			}
640

641
			/*
642
			 * Don't accept excess data.  The spec doesn't say
643
			 * what to do in this case.  We'll ignore the error.
644
			 */
645
			amount = MIN(amount, bulk_ctxt->bulk_out_length);
646

647
			// Don't write a partial block.
648
			amount -= (amount & 511);
649
			if (amount == 0)
650
				goto empty_write;
651

652
			// Perform the write.
653
			if (!sdmmc_storage_write(ums->lun.storage, ums->lun.offset + lba_offset,
654
				amount >> UMS_DISK_LBA_SHIFT, (u8 *)bulk_ctxt->bulk_out_buf))
655
				amount = 0;
656

657
DPRINTF("file write %X @ %X\n", amount, lba_offset);
658

659
			lba_offset           += amount >> UMS_DISK_LBA_SHIFT;
660
			amount_left_to_write -= amount;
661
			ums->residue         -= amount;
662

663
			// If an error occurred, report it and its position.
664
			if (!amount)
665
			{
666
				ums->set_text(ums->label, "#FFDD00 Error:# SDMMC Write!");
667
				ums->lun.sense_data      = SS_WRITE_ERROR;
668
				ums->lun.sense_data_info = lba_offset;
669
				ums->lun.info_valid      = 1;
670
				break;
671
			}
672

673
 empty_write:
674
			// Did the host decide to stop early?
675
			if (bulk_ctxt->bulk_out_length_actual < bulk_ctxt->bulk_out_length)
676
			{
677
				ums->set_text(ums->label, "#FFDD00 Error:# Empty Write!");
678
				ums->short_packet_received = 1;
679
				break;
680
			}
681
		}
682
	}
683

684
	return UMS_RES_IO_ERROR; // No default reply.
685
}
686

687
static int _scsi_verify(usbd_gadget_ums_t *ums, bulk_ctxt_t *bulk_ctxt)
688
{
689
	// Check that start LBA is past the end sector offset.
690
	u32 lba_offset = get_array_be_to_le32(&ums->cmnd[2]);
691
	if (lba_offset >= ums->lun.num_sectors)
692
	{
693
		ums->set_text(ums->label, "#FF8000 Warn:# Verif - Out of range! Host notified.");
694
		ums->lun.sense_data = SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
695

696
		return UMS_RES_INVALID_ARG;
697
	}
698

699
	// We allow DPO but we don't implement it. Check that nothing else is enabled.
700
	if (ums->cmnd[1] & ~0x10)
701
	{
702
		ums->lun.sense_data = SS_INVALID_FIELD_IN_CDB;
703

704
		return UMS_RES_INVALID_ARG;
705
	}
706

707
	u32  verification_length = get_array_be_to_le16(&ums->cmnd[7]);
708
	if (verification_length == 0)
709
		return UMS_RES_IO_ERROR; // No default reply.
710

711
	u32 amount;
712
	while (verification_length > 0)
713
	{
714

715
		// Limit to EP buffer size and end sector offset.
716
		amount = MIN(verification_length, USB_EP_BUFFER_MAX_SIZE >> UMS_DISK_LBA_SHIFT);
717
		amount = MIN(amount, ums->lun.num_sectors - lba_offset);
718
		if (amount == 0) {
719
			ums->lun.sense_data      = SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
720
			ums->lun.sense_data_info = lba_offset;
721
			ums->lun.info_valid      = 1;
722
			break;
723
		}
724

725
		if (!sdmmc_storage_read(ums->lun.storage, ums->lun.offset + lba_offset, amount, bulk_ctxt->bulk_in_buf))
726
			amount = 0;
727

728
DPRINTF("File read %X @ %X\n", amount, lba_offset);
729

730
		if (!amount)
731
		{
732
			ums->set_text(ums->label, "#FFDD00 Error:# File verify!");
733
			ums->lun.sense_data      = SS_UNRECOVERED_READ_ERROR;
734
			ums->lun.sense_data_info = lba_offset;
735
			ums->lun.info_valid      = 1;
736
			break;
737
		}
738
		lba_offset += amount;
739
		verification_length -= amount;
740
	}
741
	return UMS_RES_OK;
742
}
743

744
static int _scsi_inquiry(usbd_gadget_ums_t *ums, bulk_ctxt_t *bulk_ctxt)
745
{
746
	u8 *buf = (u8 *)bulk_ctxt->bulk_in_buf;
747

748
	memset(buf, 0, 36);
749

750
	// Enable Vital Product Data (EVPD) and Unit Serial Number.
751
	if (ums->cmnd[1] == 1 && ums->cmnd[2] == 0x80)
752
	{
753
		buf[0] = 0;
754
		buf[1] = ums->cmnd[2];
755
		buf[2] = 0;
756
		buf[3] = 20;  // Additional length.
757

758
		buf += 4;
759
		s_printf((char *)buf, "%04X%s",
760
			ums->lun.storage->cid.serial, ums->lun.type == MMC_SD ? " SD " : " eMMC ");
761

762
		switch (ums->lun.partition)
763
		{
764
		case 0:
765
			strcpy((char *)buf + strlen((char *)buf), "RAW");
766
			break;
767
		case EMMC_GPP + 1:
768
			s_printf((char *)buf + strlen((char *)buf), "GPP");
769
			break;
770
		case EMMC_BOOT0 + 1:
771
			s_printf((char *)buf + strlen((char *)buf), "BOOT0");
772
			break;
773
		case EMMC_BOOT1 + 1:
774
			s_printf((char *)buf + strlen((char *)buf), "BOOT1");
775
			break;
776
		}
777

778
		for (u32 i = strlen((char *)buf); i < 20; i++)
779
			buf[i] = ' ';
780

781
		return 24;
782
	}
783
	else /* if (ums->cmnd[1] == 0 && ums->cmnd[2] == 0) */ // Standard inquiry.
784
	{
785
		buf[0] = SCSI_TYPE_DISK;
786
		buf[1] = ums->lun.removable ? 0x80 : 0;
787
		buf[2] = 6;  // ANSI INCITS 351-2001 (SPC-2).////////SPC2: 4, SPC4: 6
788
		buf[3] = 2;  // SCSI-2 INQUIRY data format.
789
		buf[4] = 31; // Additional length.
790
		// buf5-7: No special options.
791

792
		// Vendor ID. Max 8 chars.
793
		buf += 8;
794
		strcpy((char *)buf, "hekate");
795

796
		// Product ID. Max 16 chars.
797
		buf += 8;
798
		switch (ums->lun.partition)
799
		{
800
		case 0:
801
			s_printf((char *)buf, "%s", "SD RAW");
802
			break;
803
		case EMMC_GPP + 1:
804
			s_printf((char *)buf, "%s%s",
805
				ums->lun.type == MMC_SD ? "SD " : "eMMC ", "GPP");
806
			break;
807
		case EMMC_BOOT0 + 1:
808
			s_printf((char *)buf, "%s%s",
809
				ums->lun.type == MMC_SD ? "SD " : "eMMC ", "BOOT0");
810
			break;
811
		case EMMC_BOOT1 + 1:
812
			s_printf((char *)buf, "%s%s",
813
				ums->lun.type == MMC_SD ? "SD " : "eMMC ", "BOOT1");
814
			break;
815
		}
816

817
		// Rev ID. Max 4 chars.
818
		buf += 16;
819
		strcpy((char *)buf, "1.00");
820

821
		return 36;
822
	}
823
}
824

825
static int _scsi_request_sense(usbd_gadget_ums_t *ums, bulk_ctxt_t *bulk_ctxt)
826
{
827
	u8 *buf = (u8 *)bulk_ctxt->bulk_in_buf;
828
	u32 sd, sdinfo;
829
	int valid;
830

831
	sd = ums->lun.sense_data;
832
	sdinfo = ums->lun.sense_data_info;
833
	valid = ums->lun.info_valid << 7;
834
	ums->lun.sense_data = SS_NO_SENSE;
835
	ums->lun.sense_data_info = 0;
836
	ums->lun.info_valid = 0;
837

838
	memset(buf, 0, 18);
839
	buf[0]  = valid | 0x70; // Valid, current error.
840
	buf[2]  = SK(sd);
841
	put_array_le_to_be32(sdinfo, &buf[3]); // Sense information.
842
	buf[7]  = 18 - 8; // Additional sense length.
843
	buf[12] = ASC(sd);
844
	buf[13] = ASCQ(sd);
845

846
	return 18;
847
}
848

849
static int _scsi_read_capacity(usbd_gadget_ums_t *ums, bulk_ctxt_t *bulk_ctxt)
850
{
851
	u8 *buf = (u8 *)bulk_ctxt->bulk_in_buf;
852
	u32 lba = get_array_be_to_le32(&ums->cmnd[2]);
853
	int pmi = ums->cmnd[8];
854

855
	// Check the PMI and LBA fields.
856
	if (pmi > 1 || (pmi == 0 && lba != 0))
857
	{
858
		ums->lun.sense_data = SS_INVALID_FIELD_IN_CDB;
859

860
		return UMS_RES_INVALID_ARG;
861
	}
862

863
	put_array_le_to_be32(ums->lun.num_sectors - 1, &buf[0]); // Max logical block.
864
	put_array_le_to_be32(UMS_DISK_LBA_SIZE, &buf[4]);        // Block length.
865

866
	return 8;
867
}
868

869
static int _scsi_log_sense(usbd_gadget_ums_t *ums, bulk_ctxt_t *bulk_ctxt)
870
{
871
	u8  *buf = (u8 *)bulk_ctxt->bulk_in_buf;
872
	u8  *buf0 = buf;
873
	bool valid_page = false;
874

875
	u8 pc = ums->cmnd[2] >> 6;
876
	u8 page_code = ums->cmnd[2] & 0x3F;
877
	u8 sub_page_code = ums->cmnd[3];
878

879
	if (ums->cmnd[1] & 1)
880
	{
881
		ums->lun.sense_data = SS_SAVING_PARAMETERS_NOT_SUPPORTED;
882

883
		return UMS_RES_INVALID_ARG;
884
	}
885

886
	if (pc != 1) // Current cumulative values.
887
	{
888
		ums->lun.sense_data = SS_INVALID_FIELD_IN_CDB;
889

890
		return UMS_RES_INVALID_ARG;
891
	}
892

893
	memset(buf, 0, 8);
894
	if (page_code == 0x00 && !sub_page_code) // Supported pages.
895
	{
896
		valid_page = true;
897
		buf[0] = 0x00; // Page code.
898
		buf += 4;
899

900
		buf[0] = 0x00; // Page 0.
901
		buf[1] = 0x0D; // Page 1.
902

903
		buf += 2;
904
	}
905
	else if (page_code == 0x0d && !sub_page_code) // Temperature.
906
	{
907
		valid_page = true;
908
		buf[0] = 0x0D;
909
		buf += 4;
910

911
		put_array_le_to_be16(0, &buf[0]); // Param code.
912
		buf[2] = 1;  // Param control byte.
913
		buf[3] = 2;  // Param length.
914
		buf[4] = 0;  // Reserved.
915
		buf[5] = 35; // Temperature (C) current (PCB here).
916

917
		put_array_le_to_be16(0, &buf[6]); // PARAMETER CODE
918
		buf[8] = 1;   // Param control byte.
919
		buf[9] = 2;   // Param length.
920
		buf[10] = 0;  // Reserved.
921
		buf[11] = 60; // Temperature (C) reference.
922

923
		buf += 12;
924
	}
925

926
	// Check that a valid page mode data length was requested.
927
	u32 len = buf - buf0;
928
	if (!valid_page)
929
	{
930
		ums->lun.sense_data = SS_INVALID_FIELD_IN_CDB;
931

932
		return UMS_RES_INVALID_ARG;
933
	}
934

935
	put_array_le_to_be16(len - 4, &buf0[2]);
936

937
	return len;
938
}
939

940
static int _scsi_mode_sense(usbd_gadget_ums_t *ums, bulk_ctxt_t *bulk_ctxt)
941
{
942
	u8  *buf = (u8 *)bulk_ctxt->bulk_in_buf;
943
	u8  *buf0 = buf;
944
	bool valid_page = false;
945

946
	u8 pc = ums->cmnd[2] >> 6;
947
	u8 page_code = ums->cmnd[2] & 0x3F;
948
	bool changeable_values = pc == 1;
949
	bool all_pages = page_code == 0x3F;
950

951
	if ((ums->cmnd[1] & ~0x08) != 0) // Mask away DBD.
952
	{
953
		ums->lun.sense_data = SS_INVALID_FIELD_IN_CDB;
954

955
		return UMS_RES_INVALID_ARG;
956
	}
957

958
	if (pc == 3)
959
	{
960
		ums->lun.sense_data = SS_SAVING_PARAMETERS_NOT_SUPPORTED;
961

962
		return UMS_RES_INVALID_ARG;
963
	}
964

965
	/* Write the mode parameter header.  Fixed values are: default
966
	 * medium type, no cache control (DPOFUA), and no block descriptors.
967
	 * The only variable value is the WriteProtect bit.  We will fill in
968
	 * the mode data length later. */
969
	memset(buf, 0, 8);
970
	if (ums->cmnd[0] == SC_MODE_SENSE_6)
971
	{
972
		buf[2] = (ums->lun.ro ? 0x80 : 0x00); // WP, DPOFUA.
973
		buf += 4;
974
	}
975
	else // SC_MODE_SENSE_10.
976
	{
977
		buf[3] = (ums->lun.ro ? 0x80 : 0x00); // WP, DPOFUA.
978
		buf += 8;
979
	}
980

981
	// The only page we support is the Caching page.
982
	// What about x1C
983
	if (page_code == 0x08 || all_pages)
984
	{
985
		valid_page = true;
986
		buf[0] = 0x08; // Page code.
987
		buf[1] = 18;   // Page length.
988
		memset(buf + 2, 0, 18); // Set all parameters to 0.
989

990
		// None of the fields are changeable.
991
		if (!changeable_values)
992
		{
993
			// Write Cache enable, Read Cache not disabled, Multiplication Factor off.
994
			buf[2] = 0x04;
995

996
			// Multiplication Factor is disabled, so all values below are 1x LBA.
997
			put_array_le_to_be16(0xFFFF, &buf[4]);  // Disable Prefetch if >32MB.
998
			put_array_le_to_be16(0x0000, &buf[6]);  // Minimum Prefetch 0MB.
999
			put_array_le_to_be16(0xFFFF, &buf[8]);  // Maximum Prefetch 32MB.
1000
			put_array_le_to_be16(0xFFFF, &buf[10]); // Maximum Prefetch ceiling 32MB.
1001
		}
1002

1003
		buf += 20;
1004
	}
1005

1006
	// Check that a valid page mode data length was requested.
1007
	u32 len = buf - buf0;
1008
	if (!valid_page)
1009
	{
1010
		ums->lun.sense_data = SS_INVALID_FIELD_IN_CDB;
1011

1012
		return UMS_RES_INVALID_ARG;
1013
	}
1014

1015
	//  Store the mode data length.
1016
	if (ums->cmnd[0] == SC_MODE_SENSE_6)
1017
		buf0[0] = len - 1;
1018
	else
1019
		put_array_le_to_be16(len - 2, buf0);
1020

1021
	return len;
1022
}
1023

1024
static int _scsi_start_stop(usbd_gadget_ums_t *ums)
1025
{
1026
	int loej, start;
1027

1028
	if (!ums->lun.removable)
1029
	{
1030
		ums->lun.sense_data = SS_INVALID_COMMAND;
1031

1032
		return UMS_RES_INVALID_ARG;
1033
	}
1034
	else if ((ums->cmnd[1] & ~0x01) != 0 || // Mask away Immed.
1035
		(ums->cmnd[4] & ~0x03) != 0)        // Mask LoEj, Start.
1036
	{
1037
		ums->lun.sense_data = SS_INVALID_FIELD_IN_CDB;
1038

1039
		return UMS_RES_INVALID_ARG;
1040
	}
1041

1042
	loej  = ums->cmnd[4] & 0x02;
1043
	start = ums->cmnd[4] & 0x01;
1044

1045
	// We do not support re-mounting.
1046
	if (start)
1047
	{
1048
		if (ums->lun.unmounted)
1049
		{
1050
			ums->lun.sense_data = SS_MEDIUM_NOT_PRESENT;
1051

1052
			return UMS_RES_INVALID_ARG;
1053
		}
1054

1055
		return UMS_RES_OK;
1056
	}
1057

1058
	// Check if we are allowed to unload the media.
1059
	if (ums->lun.prevent_medium_removal)
1060
	{
1061
		ums->set_text(ums->label, "#C7EA46 Status:# Unload attempt prevented");
1062
		ums->lun.sense_data = SS_MEDIUM_REMOVAL_PREVENTED;
1063

1064
		return UMS_RES_INVALID_ARG;
1065
	}
1066

1067
	if (!loej)
1068
		return UMS_RES_OK;
1069

1070
	// Unmount means we exit UMS because of ejection.
1071
	ums->lun.unmounted = 1;
1072

1073
	return UMS_RES_OK;
1074
}
1075

1076
static int _scsi_prevent_allow_removal(usbd_gadget_ums_t *ums)
1077
{
1078
	int prevent;
1079

1080
	if (!ums->lun.removable)
1081
	{
1082
		ums->lun.sense_data = SS_INVALID_COMMAND;
1083

1084
		return UMS_RES_INVALID_ARG;
1085
	}
1086

1087
	prevent = ums->cmnd[4] & 0x01;
1088
	if ((ums->cmnd[4] & ~0x01) != 0) // Mask away Prevent.
1089
	{
1090
		ums->lun.sense_data = SS_INVALID_FIELD_IN_CDB;
1091

1092
		return UMS_RES_INVALID_ARG;
1093
	}
1094

1095
	// Notify for possible unmounting?
1096
	// Normally we sync here but we do synced writes to SDMMC.
1097
	if (ums->lun.prevent_medium_removal && !prevent) { /* Do nothing */ }
1098

1099
	ums->lun.prevent_medium_removal = prevent;
1100

1101
	return UMS_RES_OK;
1102
}
1103

1104
static int _scsi_read_format_capacities(usbd_gadget_ums_t *ums, bulk_ctxt_t *bulk_ctxt)
1105
{
1106
	u8 *buf = (u8 *)bulk_ctxt->bulk_in_buf;
1107

1108
	buf[0] = buf[1] = buf[2] = 0;
1109
	buf[3] = 8; // Only the Current/Maximum Capacity Descriptor.
1110
	buf += 4;
1111

1112
	put_array_le_to_be32(ums->lun.num_sectors, &buf[0]); // Number of blocks.
1113
	put_array_le_to_be32(UMS_DISK_LBA_SIZE, &buf[4]);    // Block length.
1114
	buf[4] = 0x02; // Current capacity.
1115

1116
	return 12;
1117
}
1118

1119
// Check whether the command is properly formed and whether its data size
1120
// and direction agree with the values we already have.
1121
static int _check_scsi_cmd(usbd_gadget_ums_t *ums, u32 cmnd_size,
1122
						   enum data_direction data_dir, u32 mask,
1123
						   int needs_medium)
1124
{
1125
//const char dirletter[4] = {'u', 'o', 'i', 'n'};
1126
DPRINTF("SCSI command: %X;  Dc=%d, D%c=%X;  Hc=%d, H%c=%X\n",
1127
		ums->cmnd[0], cmnd_size, dirletter[(int)ums->data_dir],
1128
		ums->data_size_from_cmnd, ums->cmnd_size,
1129
		dirletter[(int)data_dir], ums->data_size);
1130

1131
	// We can't reply if we don't know the direction and size.
1132
	if (ums->data_size_from_cmnd == 0)
1133
		data_dir = DATA_DIR_NONE;
1134

1135
	// This is a phase error but we continue and only transfer as much we can.
1136
	if (ums->data_size < ums->data_size_from_cmnd)
1137
	{
1138
		ums->data_size_from_cmnd = ums->data_size;
1139
		ums->phase_error = 1;
1140
	}
1141

1142
	ums->residue = ums->data_size;
1143
	ums->usb_amount_left = ums->data_size;
1144

1145
	if (ums->data_dir != data_dir && ums->data_size_from_cmnd > 0)
1146
	{
1147
		ums->phase_error = 1;
1148

1149
		return UMS_RES_INVALID_ARG;
1150
	}
1151

1152
	// Cmd length verification.
1153
	if (cmnd_size != ums->cmnd_size)
1154
	{
1155

1156
		// Special case workaround for Windows and Xbox 360.
1157
		if (cmnd_size <= ums->cmnd_size)
1158
			cmnd_size = ums->cmnd_size;
1159
		else
1160
		{
1161
			ums->phase_error = 1;
1162

1163
			return UMS_RES_INVALID_ARG;
1164
		}
1165
	}
1166

1167
	// check that LUN ums->cmnd[1] >> 5 is 0 because of only one.
1168

1169
	if (ums->cmnd[0] != SC_REQUEST_SENSE)
1170
	{
1171
		ums->lun.sense_data      = SS_NO_SENSE;
1172
		ums->lun.sense_data_info = 0;
1173
		ums->lun.info_valid      = 0;
1174
	}
1175

1176
	// If a unit attention condition exists, only INQUIRY and REQUEST SENSE
1177
	// commands are allowed.
1178
	if (ums->lun.unit_attention_data != SS_NO_SENSE && ums->cmnd[0] != SC_INQUIRY &&
1179
		ums->cmnd[0] != SC_REQUEST_SENSE)
1180
	{
1181
		ums->lun.sense_data = ums->lun.unit_attention_data;
1182
		ums->lun.unit_attention_data = SS_NO_SENSE;
1183

1184
		return UMS_RES_INVALID_ARG;
1185
	}
1186

1187
	// Check that only command bytes listed in the mask are set.
1188
	ums->cmnd[1] &= 0x1F; // Mask away the LUN.
1189
	for (u32 i = 1; i < cmnd_size; ++i)
1190
	{
1191
		if (ums->cmnd[i] && !(mask & BIT(i)))
1192
		{
1193
			ums->lun.sense_data = SS_INVALID_FIELD_IN_CDB;
1194

1195
			return UMS_RES_INVALID_ARG;
1196
		}
1197
	}
1198

1199
	// If the medium isn't mounted and the command needs to access it, return an error.
1200
	if (ums->lun.unmounted && needs_medium)
1201
	{
1202
		ums->lun.sense_data = SS_MEDIUM_NOT_PRESENT;
1203

1204
		return UMS_RES_INVALID_ARG;
1205
	}
1206

1207
	return UMS_RES_OK;
1208
}
1209

1210
static int _parse_scsi_cmd(usbd_gadget_ums_t *ums, bulk_ctxt_t *bulk_ctxt)
1211
{
1212
	u32 len;
1213
	int reply = UMS_RES_INVALID_ARG;
1214

1215
	ums->phase_error = 0;
1216
	ums->short_packet_received = 0;
1217

1218
	switch (ums->cmnd[0])
1219
	{
1220
	case SC_INQUIRY:
1221
		ums->data_size_from_cmnd = ums->cmnd[4];
1222
		u32 mask = (1<<4);
1223
		if (ums->cmnd[1] == 1 && ums->cmnd[2] == 0x80) // Inquiry S/N.
1224
			mask = (1<<1) | (1<<2) | (1<<4);
1225
		reply = _check_scsi_cmd(ums, 6, DATA_DIR_TO_HOST, mask, 0);
1226
		if (reply == 0)
1227
			reply = _scsi_inquiry(ums, bulk_ctxt);
1228
		break;
1229

1230
	case SC_LOG_SENSE:
1231
		ums->data_size_from_cmnd = get_array_be_to_le16(&ums->cmnd[7]);
1232
		reply = _check_scsi_cmd(ums, 10, DATA_DIR_TO_HOST, (1<<1) | (1<<2) | (3<<7), 0);
1233
		if (reply == 0)
1234
			reply = _scsi_log_sense(ums, bulk_ctxt);
1235
		break;
1236

1237
	case SC_MODE_SELECT_6:
1238
		ums->data_size_from_cmnd = ums->cmnd[4];
1239
		reply = _check_scsi_cmd(ums, 6, DATA_DIR_FROM_HOST, (1<<1) | (1<<4), 0);
1240
		if (reply == 0)
1241
		{
1242
			// We don't support MODE SELECT.
1243
			ums->lun.sense_data = SS_INVALID_COMMAND;
1244
			reply = UMS_RES_INVALID_ARG;
1245
		}
1246
		break;
1247

1248
	case SC_MODE_SELECT_10:
1249
		ums->data_size_from_cmnd = get_array_be_to_le16(&ums->cmnd[7]);
1250
		reply = _check_scsi_cmd(ums, 10, DATA_DIR_FROM_HOST, (1<<1) | (3<<7), 0);
1251
		if (reply == 0)
1252
		{
1253
			// We don't support MODE SELECT.
1254
			ums->lun.sense_data = SS_INVALID_COMMAND;
1255
			reply = UMS_RES_INVALID_ARG;
1256
		}
1257
		break;
1258

1259
	case SC_MODE_SENSE_6:
1260
		ums->data_size_from_cmnd = ums->cmnd[4];
1261
		reply = _check_scsi_cmd(ums, 6, DATA_DIR_TO_HOST,  (1<<1) | (1<<2) | (1<<4), 0);
1262
		if (reply == 0)
1263
			reply = _scsi_mode_sense(ums, bulk_ctxt);
1264
		break;
1265

1266
	case SC_MODE_SENSE_10:
1267
		ums->data_size_from_cmnd = get_array_be_to_le16(&ums->cmnd[7]);
1268
		reply = _check_scsi_cmd(ums, 10, DATA_DIR_TO_HOST, (1<<1) | (1<<2) | (3<<7), 0);
1269
		if (reply == 0)
1270
			reply = _scsi_mode_sense(ums, bulk_ctxt);
1271
		break;
1272

1273
	case SC_PREVENT_ALLOW_MEDIUM_REMOVAL:
1274
		ums->data_size_from_cmnd = 0;
1275
		reply = _check_scsi_cmd(ums, 6, DATA_DIR_NONE, (1<<4), 0);
1276
		if (reply == 0)
1277
			reply = _scsi_prevent_allow_removal(ums);
1278
		break;
1279

1280
	case SC_READ_6:
1281
		len = ums->cmnd[4];
1282
		ums->data_size_from_cmnd = (len == 0 ? 256 : len) << UMS_DISK_LBA_SHIFT;
1283
		reply = _check_scsi_cmd(ums, 6, DATA_DIR_TO_HOST, (7<<1) | (1<<4), 1);
1284
		if (reply == 0)
1285
			reply = _scsi_read(ums, bulk_ctxt);
1286
		break;
1287

1288
	case SC_READ_10:
1289
		ums->data_size_from_cmnd = get_array_be_to_le16(&ums->cmnd[7]) << UMS_DISK_LBA_SHIFT;
1290
		reply = _check_scsi_cmd(ums, 10, DATA_DIR_TO_HOST, (1<<1) | (0xf<<2) | (3<<7), 1);
1291
		if (reply == 0)
1292
			reply = _scsi_read(ums, bulk_ctxt);
1293
		break;
1294

1295
	case SC_READ_12:
1296
		ums->data_size_from_cmnd = get_array_be_to_le32(&ums->cmnd[6]) << UMS_DISK_LBA_SHIFT;
1297
		reply = _check_scsi_cmd(ums, 12, DATA_DIR_TO_HOST, (1<<1) | (0xf<<2) | (0xf<<6), 1);
1298
		if (reply == 0)
1299
			reply = _scsi_read(ums, bulk_ctxt);
1300
		break;
1301

1302
	case SC_READ_CAPACITY:
1303
		ums->data_size_from_cmnd = 8;
1304
		reply = _check_scsi_cmd(ums, 10, DATA_DIR_TO_HOST, (0xf<<2) | (1<<8), 1);
1305
		if (reply == 0)
1306
			reply = _scsi_read_capacity(ums, bulk_ctxt);
1307
		break;
1308
	case SC_READ_FORMAT_CAPACITIES:
1309
		ums->data_size_from_cmnd = get_array_be_to_le16(&ums->cmnd[7]);
1310
		reply = _check_scsi_cmd(ums, 10, DATA_DIR_TO_HOST, (3<<7), 1);
1311
		if (reply == 0)
1312
			reply = _scsi_read_format_capacities(ums, bulk_ctxt);
1313
		break;
1314

1315
	case SC_REQUEST_SENSE:
1316
		ums->data_size_from_cmnd = ums->cmnd[4];
1317
		reply = _check_scsi_cmd(ums, 6, DATA_DIR_TO_HOST, (1<<4), 0);
1318
		if (reply == 0)
1319
			reply = _scsi_request_sense(ums, bulk_ctxt);
1320
		break;
1321

1322
	case SC_START_STOP_UNIT:
1323
		ums->data_size_from_cmnd = 0;
1324
		reply = _check_scsi_cmd(ums, 6, DATA_DIR_NONE, (1<<1) | (1<<4), 0);
1325
		if (reply == 0)
1326
			reply = _scsi_start_stop(ums);
1327
		break;
1328

1329
	case SC_SYNCHRONIZE_CACHE:
1330
		ums->data_size_from_cmnd = 0;
1331
		reply = _check_scsi_cmd(ums, 10, DATA_DIR_NONE, (0xf<<2) | (3<<7), 1);
1332
		if (reply == 0)
1333
			reply = 0; // Don't bother
1334
		break;
1335

1336
	case SC_TEST_UNIT_READY:
1337
		ums->data_size_from_cmnd = 0;
1338
		reply = _check_scsi_cmd(ums, 6, DATA_DIR_NONE, 0, 1);
1339
		break;
1340

1341
	// This command is used by Windows. We support a minimal version and BytChk must be 0.
1342
	case SC_VERIFY:
1343
		ums->data_size_from_cmnd = 0;
1344
		reply = _check_scsi_cmd(ums, 10, DATA_DIR_NONE, (1<<1) | (0xf<<2) | (3<<7), 1);
1345
		if (reply == 0)
1346
			reply = _scsi_verify(ums, bulk_ctxt);
1347
		break;
1348

1349
	case SC_WRITE_6:
1350
		len = ums->cmnd[4];
1351
		ums->data_size_from_cmnd = (len == 0 ? 256 : len) << UMS_DISK_LBA_SHIFT;
1352
		reply = _check_scsi_cmd(ums, 6, DATA_DIR_FROM_HOST, (7<<1) | (1<<4), 1);
1353
		if (reply == 0)
1354
			reply = _scsi_write(ums, bulk_ctxt);
1355
		break;
1356

1357
	case SC_WRITE_10:
1358
		ums->data_size_from_cmnd = get_array_be_to_le16(&ums->cmnd[7]) << UMS_DISK_LBA_SHIFT;
1359
		reply = _check_scsi_cmd(ums, 10, DATA_DIR_FROM_HOST, (1<<1) | (0xf<<2) | (3<<7), 1);
1360
		if (reply == 0)
1361
			reply = _scsi_write(ums, bulk_ctxt);
1362
		break;
1363

1364
	case SC_WRITE_12:
1365
		ums->data_size_from_cmnd = get_array_be_to_le32(&ums->cmnd[6]) << UMS_DISK_LBA_SHIFT;
1366
		reply = _check_scsi_cmd(ums, 12, DATA_DIR_FROM_HOST, (1<<1) | (0xf<<2) | (0xf<<6), 1);
1367
		if (reply == 0)
1368
			reply = _scsi_write(ums, bulk_ctxt);
1369
		break;
1370

1371
	// Mandatory commands that we don't implement. No need.
1372
	case SC_READ_HEADER:
1373
	case SC_READ_TOC:
1374
	case SC_FORMAT_UNIT:
1375
	case SC_RELEASE:
1376
	case SC_RESERVE:
1377
	case SC_SEND_DIAGNOSTIC:
1378
	default:
1379
		ums->data_size_from_cmnd = 0;
1380
		reply = _check_scsi_cmd(ums, ums->cmnd_size, DATA_DIR_UNKNOWN, 0xFF, 0);
1381
		if (reply == 0)
1382
		{
1383
			ums->lun.sense_data = SS_INVALID_COMMAND;
1384
			reply = UMS_RES_INVALID_ARG;
1385
		}
1386
		break;
1387
	}
1388

1389
	if (reply == UMS_RES_INVALID_ARG)
1390
		reply = 0;    // Error reply length.
1391

1392
	// Set up reply buffer for _finish_reply(). Otherwise it's already set.
1393
	if (reply >= 0 && ums->data_dir == DATA_DIR_TO_HOST)
1394
	{
1395
		reply = MIN((u32)reply, ums->data_size_from_cmnd);
1396
		bulk_ctxt->bulk_in_length = reply;
1397
		bulk_ctxt->bulk_in_buf_state = BUF_STATE_FULL;
1398
		ums->residue -= reply;
1399
	}
1400

1401
	return UMS_RES_OK;
1402
}
1403

1404
static int _pad_with_zeros(usbd_gadget_ums_t *ums, bulk_ctxt_t *bulk_ctxt)
1405
{
1406
	bulk_ctxt->bulk_in_buf_state = BUF_STATE_EMPTY; // For the first iteration.
1407
	u32 current_len_to_keep = bulk_ctxt->bulk_in_length;
1408
	ums->usb_amount_left = current_len_to_keep + ums->residue;
1409

1410
	while (ums->usb_amount_left > 0)
1411
	{
1412
		u32 nsend = MIN(ums->usb_amount_left, USB_EP_BUFFER_MAX_SIZE);
1413
		memset(bulk_ctxt->bulk_in_buf + current_len_to_keep, 0, nsend - current_len_to_keep);
1414
		bulk_ctxt->bulk_in_length = nsend;
1415
		_transfer_start(ums, bulk_ctxt, bulk_ctxt->bulk_in, USB_XFER_SYNCED_DATA);
1416
		ums->usb_amount_left -= nsend;
1417
		current_len_to_keep = 0;
1418
	}
1419

1420
	return UMS_RES_OK;
1421
}
1422

1423
static int _throw_away_data(usbd_gadget_ums_t *ums, bulk_ctxt_t *bulk_ctxt)
1424
{
1425
	if (bulk_ctxt->bulk_out_buf_state != BUF_STATE_EMPTY || ums->usb_amount_left > 0)
1426
	{
1427
		// Try to submit another request if we need one.
1428
		if (bulk_ctxt->bulk_out_buf_state == BUF_STATE_EMPTY && ums->usb_amount_left > 0)
1429
		{
1430
			u32 amount = MIN(ums->usb_amount_left, USB_EP_BUFFER_MAX_SIZE);
1431

1432
			bulk_ctxt->bulk_out_length = amount;
1433
			_transfer_start(ums, bulk_ctxt, bulk_ctxt->bulk_out, USB_XFER_SYNCED_DATA);
1434
			ums->usb_amount_left -= amount;
1435

1436
			return UMS_RES_OK;
1437
		}
1438

1439
		// Throw away the data in a filled buffer.
1440
		if (bulk_ctxt->bulk_out_buf_state == BUF_STATE_FULL)
1441
			bulk_ctxt->bulk_out_buf_state = BUF_STATE_EMPTY;
1442

1443
		// A short packet or an error ends everything.
1444
		if (bulk_ctxt->bulk_out_length_actual != bulk_ctxt->bulk_out_length ||
1445
			bulk_ctxt->bulk_out_status != USB_RES_OK)
1446
		{
1447
			raise_exception(ums, UMS_STATE_ABORT_BULK_OUT);
1448
			return UMS_RES_PROT_FATAL;
1449
		}
1450
	}
1451
	return UMS_RES_OK;
1452
}
1453

1454
static int _finish_reply(usbd_gadget_ums_t *ums, bulk_ctxt_t *bulk_ctxt)
1455
{
1456
	int rc = UMS_RES_OK;
1457

1458
	switch (ums->data_dir) {
1459
	case DATA_DIR_NONE:
1460
		break; // Nothing to send.
1461

1462
	// If this is a CB or CBI with an unknown command, we mustn't
1463
	// try to send or receive any data. Stall if we can and wait reset.
1464
	case DATA_DIR_UNKNOWN:
1465
		if (ums->can_stall)
1466
		{
1467
			_set_ep_stall(bulk_ctxt->bulk_out);
1468
			rc = _set_ep_stall(bulk_ctxt->bulk_in);
1469
			ums->set_text(ums->label, "#FFDD00 Error:# Direction unknown. Stalled both EP!");
1470
		} // Else do nothing.
1471
		break;
1472

1473
	// All but the last buffer of data have already been sent.
1474
	case DATA_DIR_TO_HOST:
1475
		if (ums->data_size)
1476
		{
1477
			// If there's no residue, simply send the last buffer.
1478
			if (!ums->residue)
1479
			{
1480
				_transfer_start(ums, bulk_ctxt, bulk_ctxt->bulk_in, USB_XFER_SYNCED_DATA);
1481

1482
			/* For Bulk-only, if we're allowed to stall then send the
1483
			 * short packet and halt the bulk-in endpoint.  If we can't
1484
			 * stall, pad out the remaining data with 0's. */
1485
			}
1486
			else if (ums->can_stall)
1487
			{
1488
				_transfer_start(ums, bulk_ctxt, bulk_ctxt->bulk_in, USB_XFER_SYNCED_DATA);
1489
				rc = _set_ep_stall(bulk_ctxt->bulk_in);
1490
				ums->set_text(ums->label, "#FFDD00 Error:# Residue. Stalled EP IN!");
1491
			}
1492
			else
1493
				rc = _pad_with_zeros(ums, bulk_ctxt);
1494
		}
1495

1496
		// In case we used SDMMC transfer, reset the buffer address.
1497
		_reset_buffer(bulk_ctxt, bulk_ctxt->bulk_in);
1498
		break;
1499

1500
	// We have processed all we want from the data the host has sent.
1501
	// There may still be outstanding bulk-out requests.
1502
	case DATA_DIR_FROM_HOST:
1503
		if (ums->residue)
1504
		{
1505
			if (ums->short_packet_received) // Did the host stop sending unexpectedly early?
1506
			{
1507
				raise_exception(ums, UMS_STATE_ABORT_BULK_OUT);
1508
				rc = UMS_RES_PROT_FATAL;
1509
			}
1510
			else // We can't stall. Read in the excess data and throw it away.
1511
				rc = _throw_away_data(ums, bulk_ctxt);
1512
		}
1513

1514
		break;
1515
	}
1516

1517
	return rc;
1518
}
1519

1520
/*
1521
 * Medium ejection heuristics.
1522
 *
1523
 * Windows:
1524
 * Uses Start/Stop Unit. Only Stop with LoEj. Observed ONLY on very specific windows machines.
1525
 * Uses Prevent/Allow Medium Removal. (For big reads and ANY write.) //////Except trivial writes. Needs check with prefetch ON
1526
 * Sends Test Unit Ready every 1s at idle. (Needs 1 EP Timeout protection: 2s)
1527
 * Does not send data when ejects. In the case it does,
1528
 *  it loops into Request Sense and Test Unit Ready when ejects.
1529
 * Line always at SE0 and only goes in J-State when it ejects.
1530
 *
1531
 * Linux:
1532
 * Uses Start/Stop Unit. Stops with LoEj when Media prevention is off.
1533
 * Uses Prevent/Allow Medium Removal. (For big read and any write.)
1534
 * Sends Test Unit Ready every 2s at idle. (Needs 2 EP Timeouts protection: 4s)
1535
 * Loops into Request Sense and Test Unit Ready when ejects.
1536
 * Line always at SE0.
1537
 *
1538
 * Mac OS:
1539
 * Uses Start/Stop. Stops with LoEj when Allow Medium Removal is enabled.
1540
 * Uses Prevent/Allow Medium Removal. (Properly. Enables at mount and only disables it when ejects.)
1541
 * Does not send Test Unit Ready at idle. But Prevent Medium Removal is enabled.
1542
 * Loops into Request Sense and Test Unit Ready when ejects.
1543
 * Line always at SE0.
1544
 */
1545

1546
static int _received_cbw(usbd_gadget_ums_t *ums, bulk_ctxt_t *bulk_ctxt)
1547
{
1548
	// Was this a real packet?  Should it be ignored?
1549
	if (bulk_ctxt->bulk_out_status || bulk_ctxt->bulk_out_ignore || ums->lun.unmounted)
1550
	{
1551
		if (bulk_ctxt->bulk_out_status || ums->lun.unmounted)
1552
		{
1553
			DPRINTF("USB: EP timeout (%d)\n", bulk_ctxt->bulk_out_status);
1554
			// In case we disconnected, exit UMS.
1555
			// Raise timeout if removable and didn't got a unit ready command inside 4s.
1556
			if (bulk_ctxt->bulk_out_status == USB2_ERROR_XFER_EP_DISABLED ||
1557
				(bulk_ctxt->bulk_out_status == USB_ERROR_TIMEOUT && ums->lun.removable && !ums->lun.prevent_medium_removal))
1558
			{
1559
				if (bulk_ctxt->bulk_out_status == USB_ERROR_TIMEOUT)
1560
				{
1561
					if (usb_ops.usb_device_get_port_in_sleep())
1562
					{
1563
						ums->set_text(ums->label, "#C7EA46 Status:# EP in sleep");
1564
						ums->timeouts += 14;
1565
					}
1566
					else if (!ums->xusb) // Timeout only on USB2.
1567
					{
1568
						ums->timeouts += 4;
1569
						DPRINTF("USB: EP removable\n");
1570
					}
1571
				}
1572
				else
1573
				{
1574
					gfx_printf("USB: EP disabled\n");
1575
					msleep(500);
1576
					ums->timeouts += 4;
1577
				}
1578
			}
1579

1580
			if (ums->lun.unmounted)
1581
			{
1582
				ums->set_text(ums->label, "#C7EA46 Status:# Medium unmounted");
1583
				ums->timeouts++;
1584
				if (!bulk_ctxt->bulk_out_status)
1585
					ums->timeouts += 3;
1586
			}
1587

1588
			if (ums->timeouts > 20)
1589
				raise_exception(ums, UMS_STATE_EXIT);
1590
		}
1591

1592
		if (bulk_ctxt->bulk_out_status || bulk_ctxt->bulk_out_ignore)
1593
			return UMS_RES_INVALID_ARG;
1594
	}
1595

1596
	// Clear request flag to allow a new one to be queued.
1597
	ums->cbw_req_queued = false;
1598

1599
	// Is the CBW valid?
1600
	bulk_recv_pkt_t *cbw = (bulk_recv_pkt_t *)bulk_ctxt->bulk_out_buf;
1601
	if (bulk_ctxt->bulk_out_length_actual != USB_BULK_CB_WRAP_LEN || cbw->Signature != USB_BULK_CB_SIG)
1602
	{
1603
		gfx_printf("USB: invalid CBW: len %X sig 0x%X\n", bulk_ctxt->bulk_out_length_actual, cbw->Signature);
1604

1605
		/*
1606
		 * The Bulk-only spec says we MUST stall the IN endpoint
1607
		 * (6.6.1), so it's unavoidable.  It also says we must
1608
		 * retain this state until the next reset, but there's
1609
		 * no way to tell the controller driver it should ignore
1610
		 * Clear-Feature(HALT) requests.
1611
		 *
1612
		 * We aren't required to halt the OUT endpoint; instead
1613
		 * we can simply accept and discard any data received
1614
		 * until the next reset.
1615
		 */
1616
		_wedge_bulk_in_endpoint(ums);
1617
		bulk_ctxt->bulk_out_ignore = 1;
1618
		return UMS_RES_INVALID_ARG;
1619
	}
1620

1621
	// Is the CBW meaningful?
1622
	if (cbw->Lun >= UMS_MAX_LUN || cbw->Flags & ~USB_BULK_IN_FLAG ||
1623
			cbw->Length == 0 || cbw->Length > SCSI_MAX_CMD_SZ)
1624
	{
1625
		gfx_printf("USB: non-meaningful CBW: lun = %X, flags = 0x%X, cmdlen %X\n",
1626
			cbw->Lun, cbw->Flags, cbw->Length);
1627

1628
		/* We can do anything we want here, so let's stall the
1629
		 * bulk pipes if we are allowed to. */
1630
		if (ums->can_stall)
1631
		{
1632
			_set_ep_stall(bulk_ctxt->bulk_out);
1633
			_set_ep_stall(bulk_ctxt->bulk_in);
1634
			ums->set_text(ums->label, "#FFDD00 Error:# CBW unknown - Stalled both EP!");
1635
		}
1636

1637
		return UMS_RES_INVALID_ARG;
1638
	}
1639

1640
	// Save the command for later.
1641
	ums->cmnd_size = cbw->Length;
1642
	memcpy(ums->cmnd, cbw->CDB, ums->cmnd_size);
1643

1644
	if (cbw->Flags & USB_BULK_IN_FLAG)
1645
		ums->data_dir = DATA_DIR_TO_HOST;
1646
	else
1647
		ums->data_dir = DATA_DIR_FROM_HOST;
1648

1649
	ums->data_size = cbw->DataTransferLength;
1650

1651
	if (ums->data_size == 0)
1652
		ums->data_dir = DATA_DIR_NONE;
1653

1654
	ums->lun_idx = cbw->Lun;
1655
	ums->tag = cbw->Tag;
1656

1657
	if (!ums->lun.unmounted)
1658
		ums->timeouts = 0;
1659

1660
	return UMS_RES_OK;
1661
}
1662

1663
static int _get_next_command(usbd_gadget_ums_t *ums, bulk_ctxt_t *bulk_ctxt)
1664
{
1665
	int rc = UMS_RES_OK;
1666

1667
	/* Wait for the next buffer to become available */
1668
	// while (bulk_ctxt->bulk_out_buf_state != BUF_STATE_EMPTY)
1669
	// {
1670
	// 	//wait irq.
1671
	// }
1672

1673
	bulk_ctxt->bulk_out_length = USB_BULK_CB_WRAP_LEN;
1674

1675
	// Queue a request to read a Bulk-only CBW.
1676
	if (!ums->cbw_req_queued)
1677
		_transfer_start(ums,  bulk_ctxt, bulk_ctxt->bulk_out, USB_XFER_SYNCED_CMD);
1678
	else
1679
		_transfer_finish(ums, bulk_ctxt, bulk_ctxt->bulk_out, USB_XFER_SYNCED_CMD);
1680

1681
	/*
1682
	 * On XUSB do not allow multiple requests for CBW to be done.
1683
	 * This avoids an issue with some XHCI controllers and OS combos (e.g. ASMedia and Linux/Mac OS)
1684
	 * which confuse that and concatenate an old CBW request with another write request (SCSI Write)
1685
	 * and create a babble error (transmit overflow).
1686
	 */
1687
	if (ums->xusb)
1688
		ums->cbw_req_queued = true;
1689

1690
	/* We will drain the buffer in software, which means we
1691
	 * can reuse it for the next filling.  No need to advance
1692
	 * next_buffhd_to_fill. */
1693

1694
	/* Wait for the CBW to arrive */
1695
	// while (bulk_ctxt->bulk_out_buf_state != BUF_STATE_FULL)
1696
	// {
1697
	// 	//wait irq.
1698
	// }
1699

1700
	rc = _received_cbw(ums, bulk_ctxt);
1701
	bulk_ctxt->bulk_out_buf_state = BUF_STATE_EMPTY;
1702

1703
	return rc;
1704
}
1705

1706
static void _send_status(usbd_gadget_ums_t *ums, bulk_ctxt_t *bulk_ctxt)
1707
{
1708
	u8  status = USB_STATUS_PASS;
1709
	u32 sd = ums->lun.sense_data;
1710

1711
	if (ums->phase_error)
1712
	{
1713
		ums->set_text(ums->label, "#FFDD00 Error:# Phase-error!");
1714
		status = USB_STATUS_PHASE_ERROR;
1715
		sd = SS_INVALID_COMMAND;
1716
	}
1717
	else if (sd != SS_NO_SENSE)
1718
	{
1719
		DPRINTF("USB: CMD fail\n");
1720
		status = USB_STATUS_FAIL;
1721
		DPRINTF("USB:   Sense: SK x%02X, ASC x%02X, ASCQ x%02X; info x%X\n",
1722
			SK(sd), ASC(sd), ASCQ(sd), ums->lun.sense_data_info);
1723
	}
1724

1725
	// Store and send the Bulk-only CSW.
1726
	bulk_send_pkt_t *csw = (bulk_send_pkt_t *)bulk_ctxt->bulk_in_buf;
1727

1728
	csw->Signature = USB_BULK_CS_SIG;
1729
	csw->Tag       = ums->tag;
1730
	csw->Residue   = ums->residue;
1731
	csw->Status    = status;
1732

1733
	bulk_ctxt->bulk_in_length = USB_BULK_CS_WRAP_LEN;
1734
	_transfer_start(ums, bulk_ctxt, bulk_ctxt->bulk_in, USB_XFER_SYNCED_CMD);
1735
}
1736

1737
static void _handle_exception(usbd_gadget_ums_t *ums, bulk_ctxt_t *bulk_ctxt)
1738
{
1739
	enum ums_state old_state;
1740

1741
	// Clear out the controller's fifos.
1742
	_flush_endpoint(bulk_ctxt->bulk_in);
1743
	_flush_endpoint(bulk_ctxt->bulk_out);
1744

1745
	/* Reset the I/O buffer states and pointers, the SCSI
1746
	 * state, and the exception.  Then invoke the handler. */
1747

1748
	bulk_ctxt->bulk_in_buf_state  = BUF_STATE_EMPTY;
1749
	bulk_ctxt->bulk_out_buf_state = BUF_STATE_EMPTY;
1750

1751
	old_state = ums->state;
1752

1753
	if (old_state != UMS_STATE_ABORT_BULK_OUT)
1754
	{
1755
		ums->lun.prevent_medium_removal = 0;
1756
		ums->lun.sense_data             = SS_NO_SENSE;
1757
		ums->lun.unit_attention_data    = SS_NO_SENSE;
1758
		ums->lun.sense_data_info        = 0;
1759
		ums->lun.info_valid             = 0;
1760
	}
1761

1762
	ums->state = UMS_STATE_NORMAL;
1763

1764
	// Carry out any extra actions required for the exception.
1765
	switch (old_state)
1766
	{
1767
	case UMS_STATE_NORMAL:
1768
		break;
1769
	case UMS_STATE_ABORT_BULK_OUT:
1770
		_send_status(ums, bulk_ctxt);
1771
		break;
1772

1773
	case UMS_STATE_PROTOCOL_RESET:
1774
		/* In case we were forced against our will to halt a
1775
		 * bulk endpoint, clear the halt now.  (The SuperH UDC
1776
		 * requires this.) */
1777
		if (bulk_ctxt->bulk_out_ignore)
1778
		{
1779
			bulk_ctxt->bulk_out_ignore = 0;
1780
			_clear_ep_stall(bulk_ctxt->bulk_in);
1781
		}
1782
		ums->lun.unit_attention_data = SS_RESET_OCCURRED;
1783
		break;
1784

1785
	case UMS_STATE_EXIT:
1786
		ums->state = UMS_STATE_TERMINATED;	// Stop the thread.
1787
		break;
1788

1789
	default:
1790
		break;
1791
	}
1792
}
1793

1794
static inline void _system_maintainance(usbd_gadget_ums_t *ums)
1795
{
1796
	static u32 timer_dram = 0;
1797
	static u32 timer_status_bar = 0;
1798

1799
	u32 time = get_tmr_ms();
1800

1801
	if (timer_status_bar < time)
1802
	{
1803
		ums->system_maintenance(true);
1804
		timer_status_bar = get_tmr_ms() + 30000;
1805
	}
1806
	else if (timer_dram < time)
1807
	{
1808
		minerva_periodic_training();
1809
		timer_dram = get_tmr_ms() + EMC_PERIODIC_TRAIN_MS;
1810
	}
1811
}
1812

1813
int usb_device_gadget_ums(usb_ctxt_t *usbs)
1814
{
1815
	int res = 0;
1816
	usbd_gadget_ums_t ums = {0};
1817

1818
	// Get USB Controller ops.
1819
	if (hw_get_chip_id() == GP_HIDREV_MAJOR_T210)
1820
		usb_device_get_ops(&usb_ops);
1821
	else
1822
	{
1823
		ums.xusb = true;
1824
		xusb_device_get_ops(&usb_ops);
1825
	}
1826

1827
	usbs->set_text(usbs->label, "#C7EA46 Status:# Started USB");
1828

1829
	if (usb_ops.usb_device_init())
1830
	{
1831
		usb_ops.usbd_end(false, true);
1832
		return 1;
1833
	}
1834

1835
	ums.state = UMS_STATE_NORMAL;
1836
	ums.can_stall = 0;
1837

1838
	ums.bulk_ctxt.bulk_in     = USB_EP_BULK_IN;
1839
	ums.bulk_ctxt.bulk_in_buf = (u8 *)USB_EP_BULK_IN_BUF_ADDR;
1840

1841
	ums.bulk_ctxt.bulk_out     = USB_EP_BULK_OUT;
1842
	ums.bulk_ctxt.bulk_out_buf = (u8 *)USB_EP_BULK_OUT_BUF_ADDR;
1843

1844
	// Set LUN parameters.
1845
	ums.lun.ro          = usbs->ro;
1846
	ums.lun.type        = usbs->type;
1847
	ums.lun.partition   = usbs->partition;
1848
	ums.lun.num_sectors = usbs->sectors;
1849
	ums.lun.offset      = usbs->offset;
1850
	ums.lun.removable = 1; // Always removable to force OSes to use prevent media removal.
1851
	ums.lun.unit_attention_data = SS_RESET_OCCURRED;
1852

1853
	// Set system functions
1854
	ums.label = usbs->label;
1855
	ums.set_text = usbs->set_text;
1856
	ums.system_maintenance = usbs->system_maintenance;
1857

1858
	ums.set_text(ums.label, "#C7EA46 Status:# Mounting disk");
1859

1860
	// Initialize sdmmc.
1861
	if (usbs->type == MMC_SD)
1862
	{
1863
		sd_end();
1864
		if (!sd_mount())
1865
		{
1866
			ums.set_text(ums.label, "#FFDD00 Failed to init SD!#");
1867
			res = 1;
1868
			goto init_fail;
1869
		}
1870
		sd_unmount();
1871

1872
		ums.lun.sdmmc   = &sd_sdmmc;
1873
		ums.lun.storage = &sd_storage;
1874
	}
1875
	else
1876
	{
1877
		if (!emmc_initialize(false))
1878
		{
1879
			ums.set_text(ums.label, "#FFDD00 Failed to init eMMC!#");
1880
			res = 1;
1881
			goto init_fail;
1882
		}
1883
		emmc_set_partition(ums.lun.partition - 1);
1884

1885
		ums.lun.sdmmc   = &emmc_sdmmc;
1886
		ums.lun.storage = &emmc_storage;
1887
	}
1888

1889
	ums.set_text(ums.label, "#C7EA46 Status:# Waiting for connection");
1890

1891
	// Initialize Control Endpoint.
1892
	if (usb_ops.usb_device_enumerate(USB_GADGET_UMS))
1893
		goto usb_enum_error;
1894

1895
	ums.set_text(ums.label, "#C7EA46 Status:# Waiting for LUN");
1896

1897
	if (usb_ops.usb_device_class_send_max_lun(0)) // One device for now.
1898
		goto usb_enum_error;
1899

1900
	ums.set_text(ums.label, "#C7EA46 Status:# Started UMS");
1901

1902
	// If partition sectors are not set get them from hardware.
1903
	if (!ums.lun.num_sectors)
1904
	{
1905
		if (usbs->type == MMC_EMMC && (ums.lun.partition - 1)) // eMMC BOOT0/1.
1906
			ums.lun.num_sectors = emmc_storage.ext_csd.boot_mult << 8;
1907
		else
1908
			ums.lun.num_sectors = ums.lun.storage->sec_cnt;   // eMMC GPP or SD.
1909
	}
1910

1911
	do
1912
	{
1913
		// Do DRAM training and update system tasks.
1914
		_system_maintainance(&ums);
1915

1916
		// Check for force unmount button combo.
1917
		if (btn_read_vol() == (BTN_VOL_UP | BTN_VOL_DOWN))
1918
		{
1919
			// Check if we are allowed to unload the media.
1920
			if (ums.lun.prevent_medium_removal)
1921
				ums.set_text(ums.label, "#C7EA46 Status:# Unload attempt prevented");
1922
			else
1923
				break;
1924
		}
1925

1926
		if (ums.state != UMS_STATE_NORMAL)
1927
		{
1928
			_handle_exception(&ums, &ums.bulk_ctxt);
1929
			continue;
1930
		}
1931

1932
		_handle_ep0_ctrl(&ums);
1933

1934
		if (_get_next_command(&ums, &ums.bulk_ctxt) || (ums.state > UMS_STATE_NORMAL))
1935
			continue;
1936

1937
		_handle_ep0_ctrl(&ums);
1938

1939
		_parse_scsi_cmd(&ums, &ums.bulk_ctxt);
1940

1941
		if (ums.state > UMS_STATE_NORMAL)
1942
			continue;
1943

1944
		_handle_ep0_ctrl(&ums);
1945

1946
		if (_finish_reply(&ums, &ums.bulk_ctxt) || (ums.state > UMS_STATE_NORMAL))
1947
			continue;
1948

1949
		_send_status(&ums, &ums.bulk_ctxt);
1950
	} while (ums.state != UMS_STATE_TERMINATED);
1951

1952
	if (ums.lun.prevent_medium_removal)
1953
		ums.set_text(ums.label, "#FFDD00 Error:# Disk unsafely ejected");
1954
	else
1955
		ums.set_text(ums.label, "#C7EA46 Status:# Disk ejected");
1956
	goto exit;
1957

1958
usb_enum_error:
1959
	ums.set_text(ums.label, "#FFDD00 Error:# Timed out or canceled!");
1960
	res = 1;
1961

1962
exit:
1963
	if (ums.lun.type == MMC_EMMC)
1964
		emmc_end();
1965

1966
init_fail:
1967
	usb_ops.usbd_end(true, false);
1968

1969
	return res;
1970
}
1971

1972