1 /*
2 * Copyright (c) 2022, sakumisu
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6 #include "usbd_core.h"
7 #include "usbd_msc.h"
8 #include "usb_scsi.h"
9 #if defined(CONFIG_USBDEV_MSC_THREAD)
10 #include "usb_osal.h"
11 #endif
12
13 #define MSD_OUT_EP_IDX 0
14 #define MSD_IN_EP_IDX 1
15
16 /* Describe EndPoints configuration */
17 static struct usbd_endpoint mass_ep_data[2];
18
19 /* MSC Bulk-only Stage */
20 enum Stage {
21 MSC_READ_CBW = 0, /* Command Block Wrapper */
22 MSC_DATA_OUT = 1, /* Data Out Phase */
23 MSC_DATA_IN = 2, /* Data In Phase */
24 MSC_SEND_CSW = 3, /* Command Status Wrapper */
25 MSC_WAIT_CSW = 4, /* Command Status Wrapper */
26 };
27
28 /* Device data structure */
29 USB_NOCACHE_RAM_SECTION struct usbd_msc_priv {
30 /* state of the bulk-only state machine */
31 enum Stage stage;
32 USB_MEM_ALIGNX struct CBW cbw;
33 USB_MEM_ALIGNX struct CSW csw;
34
35 bool readonly;
36 bool popup;
37 uint8_t sKey; /* Sense key */
38 uint8_t ASC; /* Additional Sense Code */
39 uint8_t ASQ; /* Additional Sense Qualifier */
40 uint8_t max_lun;
41 uint32_t start_sector;
42 uint32_t nsectors;
43 uint16_t scsi_blk_size;
44 uint32_t scsi_blk_nbr;
45
46 USB_MEM_ALIGNX uint8_t block_buffer[CONFIG_USBDEV_MSC_BLOCK_SIZE];
47
48 #if defined(CONFIG_USBDEV_MSC_THREAD)
49 usb_osal_mq_t usbd_msc_mq;
50 usb_osal_thread_t usbd_msc_thread;
51 uint32_t nbytes;
52 #endif
53 } g_usbd_msc;
54
usbd_msc_reset(void)55 static void usbd_msc_reset(void)
56 {
57 g_usbd_msc.stage = MSC_READ_CBW;
58 g_usbd_msc.readonly = false;
59 }
60
msc_storage_class_interface_request_handler(struct usb_setup_packet * setup,uint8_t ** data,uint32_t * len)61 static int msc_storage_class_interface_request_handler(struct usb_setup_packet *setup, uint8_t **data, uint32_t *len)
62 {
63 USB_LOG_DBG("MSC Class request: "
64 "bRequest 0x%02x\r\n",
65 setup->bRequest);
66
67 switch (setup->bRequest) {
68 case MSC_REQUEST_RESET:
69 usbd_msc_reset();
70 break;
71
72 case MSC_REQUEST_GET_MAX_LUN:
73 (*data)[0] = g_usbd_msc.max_lun;
74 *len = 1;
75 break;
76
77 default:
78 USB_LOG_WRN("Unhandled MSC Class bRequest 0x%02x\r\n", setup->bRequest);
79 return -1;
80 }
81
82 return 0;
83 }
84
msc_storage_notify_handler(uint8_t event,void * arg)85 void msc_storage_notify_handler(uint8_t event, void *arg)
86 {
87 switch (event) {
88 case USBD_EVENT_RESET:
89 usbd_msc_reset();
90 break;
91 case USBD_EVENT_CONFIGURED:
92 USB_LOG_DBG("Start reading cbw\r\n");
93 usbd_ep_start_read(mass_ep_data[MSD_OUT_EP_IDX].ep_addr, (uint8_t *)&g_usbd_msc.cbw, USB_SIZEOF_MSC_CBW);
94 break;
95
96 default:
97 break;
98 }
99 }
100
usbd_msc_bot_abort(void)101 static void usbd_msc_bot_abort(void)
102 {
103 if ((g_usbd_msc.cbw.bmFlags == 0) && (g_usbd_msc.cbw.dDataLength != 0)) {
104 usbd_ep_set_stall(mass_ep_data[MSD_OUT_EP_IDX].ep_addr);
105 }
106 usbd_ep_set_stall(mass_ep_data[MSD_IN_EP_IDX].ep_addr);
107 usbd_ep_start_read(mass_ep_data[0].ep_addr, (uint8_t *)&g_usbd_msc.cbw, USB_SIZEOF_MSC_CBW);
108 }
109
usbd_msc_send_csw(uint8_t CSW_Status)110 static void usbd_msc_send_csw(uint8_t CSW_Status)
111 {
112 g_usbd_msc.csw.dSignature = MSC_CSW_Signature;
113 g_usbd_msc.csw.bStatus = CSW_Status;
114
115 /* updating the State Machine , so that we wait CSW when this
116 * transfer is complete, ie when we get a bulk in callback
117 */
118 g_usbd_msc.stage = MSC_WAIT_CSW;
119
120 USB_LOG_DBG("Send csw\r\n");
121 usbd_ep_start_write(mass_ep_data[MSD_IN_EP_IDX].ep_addr, (uint8_t *)&g_usbd_msc.csw, sizeof(struct CSW));
122 }
123
usbd_msc_send_info(uint8_t * buffer,uint8_t size)124 static void usbd_msc_send_info(uint8_t *buffer, uint8_t size)
125 {
126 size = MIN(size, g_usbd_msc.cbw.dDataLength);
127
128 /* updating the State Machine , so that we send CSW when this
129 * transfer is complete, ie when we get a bulk in callback
130 */
131 g_usbd_msc.stage = MSC_SEND_CSW;
132
133 usbd_ep_start_write(mass_ep_data[MSD_IN_EP_IDX].ep_addr, buffer, size);
134
135 g_usbd_msc.csw.dDataResidue -= size;
136 g_usbd_msc.csw.bStatus = CSW_STATUS_CMD_PASSED;
137 }
138
139 static bool SCSI_processWrite(uint32_t nbytes);
140 static bool SCSI_processRead(void);
141
142 /**
143 * @brief SCSI_SetSenseData
144 * Load the last error code in the error list
145 * @param sKey: Sense Key
146 * @param ASC: Additional Sense Code
147 * @retval none
148
149 */
SCSI_SetSenseData(uint32_t KCQ)150 static void SCSI_SetSenseData(uint32_t KCQ)
151 {
152 g_usbd_msc.sKey = (uint8_t)(KCQ >> 16);
153 g_usbd_msc.ASC = (uint8_t)(KCQ >> 8);
154 g_usbd_msc.ASQ = (uint8_t)(KCQ);
155 }
156
157 /**
158 * @brief SCSI Command list
159 *
160 */
161
SCSI_testUnitReady(uint8_t ** data,uint32_t * len)162 static bool SCSI_testUnitReady(uint8_t **data, uint32_t *len)
163 {
164 if (g_usbd_msc.cbw.dDataLength != 0U) {
165 SCSI_SetSenseData(SCSI_KCQIR_INVALIDCOMMAND);
166 return false;
167 }
168 *data = NULL;
169 *len = 0;
170 return true;
171 }
172
SCSI_requestSense(uint8_t ** data,uint32_t * len)173 static bool SCSI_requestSense(uint8_t **data, uint32_t *len)
174 {
175 uint8_t data_len = SCSIRESP_FIXEDSENSEDATA_SIZEOF;
176 if (g_usbd_msc.cbw.dDataLength == 0U) {
177 SCSI_SetSenseData(SCSI_KCQIR_INVALIDCOMMAND);
178 return false;
179 }
180
181 if (g_usbd_msc.cbw.CB[4] < SCSIRESP_FIXEDSENSEDATA_SIZEOF) {
182 data_len = g_usbd_msc.cbw.CB[4];
183 }
184
185 uint8_t request_sense[SCSIRESP_FIXEDSENSEDATA_SIZEOF] = {
186 0x70,
187 0x00,
188 0x00, /* Sense Key */
189 0x00,
190 0x00,
191 0x00,
192 0x00,
193 SCSIRESP_FIXEDSENSEDATA_SIZEOF - 8,
194 0x00,
195 0x00,
196 0x00,
197 0x00,
198 0x00, /* Additional Sense Code */
199 0x00, /* Additional Sense Request */
200 0x00,
201 0x00,
202 0x00,
203 0x00,
204 };
205
206 request_sense[2] = g_usbd_msc.sKey;
207 request_sense[12] = g_usbd_msc.ASC;
208 request_sense[13] = g_usbd_msc.ASQ;
209 #if 0
210 request_sense[ 2] = 0x06; /* UNIT ATTENTION */
211 request_sense[12] = 0x28; /* Additional Sense Code: Not ready to ready transition */
212 request_sense[13] = 0x00; /* Additional Sense Code Qualifier */
213 #endif
214 #if 0
215 request_sense[ 2] = 0x02; /* NOT READY */
216 request_sense[12] = 0x3A; /* Additional Sense Code: Medium not present */
217 request_sense[13] = 0x00; /* Additional Sense Code Qualifier */
218 #endif
219 #if 0
220 request_sense[ 2] = 0x05; /* ILLEGAL REQUEST */
221 request_sense[12] = 0x20; /* Additional Sense Code: Invalid command */
222 request_sense[13] = 0x00; /* Additional Sense Code Qualifier */
223 #endif
224 #if 0
225 request_sense[ 2] = 0x00; /* NO SENSE */
226 request_sense[12] = 0x00; /* Additional Sense Code: No additional code */
227 request_sense[13] = 0x00; /* Additional Sense Code Qualifier */
228 #endif
229
230 memcpy(*data, (uint8_t *)request_sense, data_len);
231 *len = data_len;
232 return true;
233 }
234
SCSI_inquiry(uint8_t ** data,uint32_t * len)235 static bool SCSI_inquiry(uint8_t **data, uint32_t *len)
236 {
237 uint8_t data_len = SCSIRESP_INQUIRY_SIZEOF;
238
239 uint8_t inquiry00[6] = {
240 0x00,
241 0x00,
242 0x00,
243 (0x06 - 4U),
244 0x00,
245 0x80
246 };
247
248 /* USB Mass storage VPD Page 0x80 Inquiry Data for Unit Serial Number */
249 uint8_t inquiry80[8] = {
250 0x00,
251 0x80,
252 0x00,
253 0x08,
254 0x20, /* Put Product Serial number */
255 0x20,
256 0x20,
257 0x20
258 };
259
260 uint8_t inquiry[SCSIRESP_INQUIRY_SIZEOF] = {
261 /* 36 */
262
263 /* LUN 0 */
264 0x00,
265 0x80,
266 0x02,
267 0x02,
268 (SCSIRESP_INQUIRY_SIZEOF - 5),
269 0x00,
270 0x00,
271 0x00,
272 ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', /* Manufacturer : 8 bytes */
273 ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', /* Product : 16 Bytes */
274 ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ',
275 ' ', ' ', ' ', ' ' /* Version : 4 Bytes */
276 };
277
278 memcpy(&inquiry[8], CONFIG_USBDEV_MSC_MANUFACTURER_STRING, strlen(CONFIG_USBDEV_MSC_MANUFACTURER_STRING));
279 memcpy(&inquiry[16], CONFIG_USBDEV_MSC_PRODUCT_STRING, strlen(CONFIG_USBDEV_MSC_PRODUCT_STRING));
280 memcpy(&inquiry[32], CONFIG_USBDEV_MSC_VERSION_STRING, strlen(CONFIG_USBDEV_MSC_VERSION_STRING));
281
282 if (g_usbd_msc.cbw.dDataLength == 0U) {
283 SCSI_SetSenseData(SCSI_KCQIR_INVALIDCOMMAND);
284 return false;
285 }
286
287 if ((g_usbd_msc.cbw.CB[1] & 0x01U) != 0U) { /* Evpd is set */
288 if (g_usbd_msc.cbw.CB[2] == 0U) { /* Request for Supported Vital Product Data Pages*/
289 data_len = 0x06;
290 memcpy(*data, (uint8_t *)inquiry00, data_len);
291 } else if (g_usbd_msc.cbw.CB[2] == 0x80U) { /* Request for VPD page 0x80 Unit Serial Number */
292 data_len = 0x08;
293 memcpy(*data, (uint8_t *)inquiry80, data_len);
294 } else { /* Request Not supported */
295 SCSI_SetSenseData(SCSI_KCQIR_INVALIDFIELDINCBA);
296 return false;
297 }
298 } else {
299 if (g_usbd_msc.cbw.CB[4] < SCSIRESP_INQUIRY_SIZEOF) {
300 data_len = g_usbd_msc.cbw.CB[4];
301 }
302 memcpy(*data, (uint8_t *)inquiry, data_len);
303 }
304
305 *len = data_len;
306 return true;
307 }
308
SCSI_startStopUnit(uint8_t ** data,uint32_t * len)309 static bool SCSI_startStopUnit(uint8_t **data, uint32_t *len)
310 {
311 if (g_usbd_msc.cbw.dDataLength != 0U) {
312 SCSI_SetSenseData(SCSI_KCQIR_INVALIDCOMMAND);
313 return false;
314 }
315
316 if ((g_usbd_msc.cbw.CB[4] & 0x3U) == 0x1U) /* START=1 */
317 {
318 //SCSI_MEDIUM_UNLOCKED;
319 } else if ((g_usbd_msc.cbw.CB[4] & 0x3U) == 0x2U) /* START=0 and LOEJ Load Eject=1 */
320 {
321 //SCSI_MEDIUM_EJECTED;
322 g_usbd_msc.popup = true;
323 } else if ((g_usbd_msc.cbw.CB[4] & 0x3U) == 0x3U) /* START=1 and LOEJ Load Eject=1 */
324 {
325 //SCSI_MEDIUM_UNLOCKED;
326 } else {
327 }
328
329 *data = NULL;
330 *len = 0;
331 return true;
332 }
333
SCSI_preventAllowMediaRemoval(uint8_t ** data,uint32_t * len)334 static bool SCSI_preventAllowMediaRemoval(uint8_t **data, uint32_t *len)
335 {
336 if (g_usbd_msc.cbw.dDataLength != 0U) {
337 SCSI_SetSenseData(SCSI_KCQIR_INVALIDCOMMAND);
338 return false;
339 }
340 if (g_usbd_msc.cbw.CB[4] == 0U) {
341 //SCSI_MEDIUM_UNLOCKED;
342 } else {
343 //SCSI_MEDIUM_LOCKED;
344 }
345 *data = NULL;
346 *len = 0;
347 return true;
348 }
349
SCSI_modeSense6(uint8_t ** data,uint32_t * len)350 static bool SCSI_modeSense6(uint8_t **data, uint32_t *len)
351 {
352 uint8_t data_len = 4;
353 if (g_usbd_msc.cbw.dDataLength == 0U) {
354 SCSI_SetSenseData(SCSI_KCQIR_INVALIDCOMMAND);
355 return false;
356 }
357 if (g_usbd_msc.cbw.CB[4] < SCSIRESP_MODEPARAMETERHDR6_SIZEOF) {
358 data_len = g_usbd_msc.cbw.CB[4];
359 }
360
361 uint8_t sense6[SCSIRESP_MODEPARAMETERHDR6_SIZEOF] = { 0x03, 0x00, 0x00, 0x00 };
362
363 if (g_usbd_msc.readonly) {
364 sense6[2] = 0x80;
365 }
366 memcpy(*data, (uint8_t *)sense6, data_len);
367 *len = data_len;
368 return true;
369 }
370
SCSI_modeSense10(uint8_t ** data,uint32_t * len)371 static bool SCSI_modeSense10(uint8_t **data, uint32_t *len)
372 {
373 uint8_t data_len = 27;
374 if (g_usbd_msc.cbw.dDataLength == 0U) {
375 SCSI_SetSenseData(SCSI_KCQIR_INVALIDCOMMAND);
376 return false;
377 }
378
379 if (g_usbd_msc.cbw.CB[8] < 27) {
380 data_len = g_usbd_msc.cbw.CB[8];
381 }
382
383 uint8_t sense10[27] = {
384 0x00,
385 0x26,
386 0x00,
387 0x00,
388 0x00,
389 0x00,
390 0x00,
391 0x00,
392 0x08,
393 0x12,
394 0x00,
395 0x00,
396 0x00,
397 0x00,
398 0x00,
399 0x00,
400 0x00,
401 0x00,
402 0x00,
403 0x00,
404 0x00,
405 0x00,
406 0x00,
407 0x00,
408 0x00,
409 0x00,
410 0x00
411 };
412
413 memcpy(*data, (uint8_t *)sense10, data_len);
414 *len = data_len;
415 return true;
416 }
417
SCSI_readFormatCapacity(uint8_t ** data,uint32_t * len)418 static bool SCSI_readFormatCapacity(uint8_t **data, uint32_t *len)
419 {
420 if (g_usbd_msc.cbw.dDataLength == 0U) {
421 SCSI_SetSenseData(SCSI_KCQIR_INVALIDCOMMAND);
422 return false;
423 }
424 uint8_t format_capacity[SCSIRESP_READFORMATCAPACITIES_SIZEOF] = {
425 0x00,
426 0x00,
427 0x00,
428 0x08, /* Capacity List Length */
429 (uint8_t)((g_usbd_msc.scsi_blk_nbr >> 24) & 0xff),
430 (uint8_t)((g_usbd_msc.scsi_blk_nbr >> 16) & 0xff),
431 (uint8_t)((g_usbd_msc.scsi_blk_nbr >> 8) & 0xff),
432 (uint8_t)((g_usbd_msc.scsi_blk_nbr >> 0) & 0xff),
433
434 0x02, /* Descriptor Code: Formatted Media */
435 0x00,
436 (uint8_t)((g_usbd_msc.scsi_blk_size >> 8) & 0xff),
437 (uint8_t)((g_usbd_msc.scsi_blk_size >> 0) & 0xff),
438 };
439
440 memcpy(*data, (uint8_t *)format_capacity, SCSIRESP_READFORMATCAPACITIES_SIZEOF);
441 *len = SCSIRESP_READFORMATCAPACITIES_SIZEOF;
442 return true;
443 }
444
SCSI_readCapacity10(uint8_t ** data,uint32_t * len)445 static bool SCSI_readCapacity10(uint8_t **data, uint32_t *len)
446 {
447 if (g_usbd_msc.cbw.dDataLength == 0U) {
448 SCSI_SetSenseData(SCSI_KCQIR_INVALIDCOMMAND);
449 return false;
450 }
451
452 uint8_t capacity10[SCSIRESP_READCAPACITY10_SIZEOF] = {
453 (uint8_t)(((g_usbd_msc.scsi_blk_nbr - 1) >> 24) & 0xff),
454 (uint8_t)(((g_usbd_msc.scsi_blk_nbr - 1) >> 16) & 0xff),
455 (uint8_t)(((g_usbd_msc.scsi_blk_nbr - 1) >> 8) & 0xff),
456 (uint8_t)(((g_usbd_msc.scsi_blk_nbr - 1) >> 0) & 0xff),
457
458 (uint8_t)((g_usbd_msc.scsi_blk_size >> 24) & 0xff),
459 (uint8_t)((g_usbd_msc.scsi_blk_size >> 16) & 0xff),
460 (uint8_t)((g_usbd_msc.scsi_blk_size >> 8) & 0xff),
461 (uint8_t)((g_usbd_msc.scsi_blk_size >> 0) & 0xff),
462 };
463
464 memcpy(*data, (uint8_t *)capacity10, SCSIRESP_READCAPACITY10_SIZEOF);
465 *len = SCSIRESP_READCAPACITY10_SIZEOF;
466 return true;
467 }
468
SCSI_read10(uint8_t ** data,uint32_t * len)469 static bool SCSI_read10(uint8_t **data, uint32_t *len)
470 {
471 if (((g_usbd_msc.cbw.bmFlags & 0x80U) != 0x80U) || (g_usbd_msc.cbw.dDataLength == 0U)) {
472 SCSI_SetSenseData(SCSI_KCQIR_INVALIDCOMMAND);
473 return false;
474 }
475
476 g_usbd_msc.start_sector = GET_BE32(&g_usbd_msc.cbw.CB[2]); /* Logical Block Address of First Block */
477 USB_LOG_DBG("lba: 0x%04x\r\n", g_usbd_msc.start_sector);
478
479 g_usbd_msc.nsectors = GET_BE16(&g_usbd_msc.cbw.CB[7]); /* Number of Blocks to transfer */
480 USB_LOG_DBG("nsectors: 0x%02x\r\n", g_usbd_msc.nsectors);
481
482 if ((g_usbd_msc.start_sector + g_usbd_msc.nsectors) > g_usbd_msc.scsi_blk_nbr) {
483 SCSI_SetSenseData(SCSI_KCQIR_LBAOUTOFRANGE);
484 USB_LOG_ERR("LBA out of range\r\n");
485 return false;
486 }
487
488 if (g_usbd_msc.cbw.dDataLength != (g_usbd_msc.nsectors * g_usbd_msc.scsi_blk_size)) {
489 USB_LOG_ERR("scsi_blk_len does not match with dDataLength\r\n");
490 return false;
491 }
492 g_usbd_msc.stage = MSC_DATA_IN;
493 #ifdef CONFIG_USBDEV_MSC_THREAD
494 usb_osal_mq_send(g_usbd_msc.usbd_msc_mq, MSC_DATA_IN);
495 return true;
496 #else
497 return SCSI_processRead();
498 #endif
499 }
500
SCSI_read12(uint8_t ** data,uint32_t * len)501 static bool SCSI_read12(uint8_t **data, uint32_t *len)
502 {
503 if (((g_usbd_msc.cbw.bmFlags & 0x80U) != 0x80U) || (g_usbd_msc.cbw.dDataLength == 0U)) {
504 SCSI_SetSenseData(SCSI_KCQIR_INVALIDCOMMAND);
505 return false;
506 }
507
508 g_usbd_msc.start_sector = GET_BE32(&g_usbd_msc.cbw.CB[2]); /* Logical Block Address of First Block */
509 USB_LOG_DBG("lba: 0x%04x\r\n", g_usbd_msc.start_sector);
510
511 g_usbd_msc.nsectors = GET_BE32(&g_usbd_msc.cbw.CB[6]); /* Number of Blocks to transfer */
512 USB_LOG_DBG("nsectors: 0x%02x\r\n", g_usbd_msc.nsectors);
513
514 if ((g_usbd_msc.start_sector + g_usbd_msc.nsectors) > g_usbd_msc.scsi_blk_nbr) {
515 SCSI_SetSenseData(SCSI_KCQIR_LBAOUTOFRANGE);
516 USB_LOG_ERR("LBA out of range\r\n");
517 return false;
518 }
519
520 if (g_usbd_msc.cbw.dDataLength != (g_usbd_msc.nsectors * g_usbd_msc.scsi_blk_size)) {
521 USB_LOG_ERR("scsi_blk_len does not match with dDataLength\r\n");
522 return false;
523 }
524 g_usbd_msc.stage = MSC_DATA_IN;
525 #ifdef CONFIG_USBDEV_MSC_THREAD
526 usb_osal_mq_send(g_usbd_msc.usbd_msc_mq, MSC_DATA_IN);
527 return true;
528 #else
529 return SCSI_processRead();
530 #endif
531 }
532
SCSI_write10(uint8_t ** data,uint32_t * len)533 static bool SCSI_write10(uint8_t **data, uint32_t *len)
534 {
535 uint32_t data_len = 0;
536 if (((g_usbd_msc.cbw.bmFlags & 0x80U) != 0x00U) || (g_usbd_msc.cbw.dDataLength == 0U)) {
537 SCSI_SetSenseData(SCSI_KCQIR_INVALIDCOMMAND);
538 return false;
539 }
540
541 g_usbd_msc.start_sector = GET_BE32(&g_usbd_msc.cbw.CB[2]); /* Logical Block Address of First Block */
542 USB_LOG_DBG("lba: 0x%04x\r\n", g_usbd_msc.start_sector);
543
544 g_usbd_msc.nsectors = GET_BE16(&g_usbd_msc.cbw.CB[7]); /* Number of Blocks to transfer */
545 USB_LOG_DBG("nsectors: 0x%02x\r\n", g_usbd_msc.nsectors);
546
547 data_len = g_usbd_msc.nsectors * g_usbd_msc.scsi_blk_size;
548 if ((g_usbd_msc.start_sector + g_usbd_msc.nsectors) > g_usbd_msc.scsi_blk_nbr) {
549 USB_LOG_ERR("LBA out of range\r\n");
550 return false;
551 }
552
553 if (g_usbd_msc.cbw.dDataLength != data_len) {
554 return false;
555 }
556 g_usbd_msc.stage = MSC_DATA_OUT;
557 data_len = MIN(data_len, CONFIG_USBDEV_MSC_BLOCK_SIZE);
558 usbd_ep_start_read(mass_ep_data[MSD_OUT_EP_IDX].ep_addr, g_usbd_msc.block_buffer, data_len);
559 return true;
560 }
561
SCSI_write12(uint8_t ** data,uint32_t * len)562 static bool SCSI_write12(uint8_t **data, uint32_t *len)
563 {
564 uint32_t data_len = 0;
565 if (((g_usbd_msc.cbw.bmFlags & 0x80U) != 0x00U) || (g_usbd_msc.cbw.dDataLength == 0U)) {
566 SCSI_SetSenseData(SCSI_KCQIR_INVALIDCOMMAND);
567 return false;
568 }
569
570 g_usbd_msc.start_sector = GET_BE32(&g_usbd_msc.cbw.CB[2]); /* Logical Block Address of First Block */
571 USB_LOG_DBG("lba: 0x%04x\r\n", g_usbd_msc.start_sector);
572
573 g_usbd_msc.nsectors = GET_BE32(&g_usbd_msc.cbw.CB[6]); /* Number of Blocks to transfer */
574 USB_LOG_DBG("nsectors: 0x%02x\r\n", g_usbd_msc.nsectors);
575
576 data_len = g_usbd_msc.nsectors * g_usbd_msc.scsi_blk_size;
577 if ((g_usbd_msc.start_sector + g_usbd_msc.nsectors) > g_usbd_msc.scsi_blk_nbr) {
578 USB_LOG_ERR("LBA out of range\r\n");
579 return false;
580 }
581
582 if (g_usbd_msc.cbw.dDataLength != data_len) {
583 return false;
584 }
585 g_usbd_msc.stage = MSC_DATA_OUT;
586 data_len = MIN(data_len, CONFIG_USBDEV_MSC_BLOCK_SIZE);
587 usbd_ep_start_read(mass_ep_data[MSD_OUT_EP_IDX].ep_addr, g_usbd_msc.block_buffer, data_len);
588 return true;
589 }
590 /* do not use verify to reduce code size */
591 #if 0
592 static bool SCSI_verify10(uint8_t **data, uint32_t *len)
593 {
594 /* Logical Block Address of First Block */
595 uint32_t lba = 0;
596 uint32_t blk_num = 0;
597
598 if ((g_usbd_msc.cbw.CB[1] & 0x02U) == 0x00U) {
599 return true;
600 }
601
602 if (((g_usbd_msc.cbw.bmFlags & 0x80U) != 0x00U) || (g_usbd_msc.cbw.dDataLength == 0U)) {
603 SCSI_SetSenseData(SCSI_KCQIR_INVALIDCOMMAND);
604 return false;
605 }
606
607 if ((g_usbd_msc.cbw.CB[1] & 0x02U) == 0x02U) {
608 SCSI_SetSenseData(SCSI_KCQIR_INVALIDFIELDINCBA);
609 return false; /* Error, Verify Mode Not supported*/
610 }
611
612 lba = GET_BE32(&g_usbd_msc.cbw.CB[2]);
613 USB_LOG_DBG("lba: 0x%x\r\n", lba);
614
615 g_usbd_msc.scsi_blk_addr = lba * g_usbd_msc.scsi_blk_size;
616
617 /* Number of Blocks to transfer */
618 blk_num = GET_BE16(&g_usbd_msc.cbw.CB[7]);
619
620 USB_LOG_DBG("num (block) : 0x%x\r\n", blk_num);
621 g_usbd_msc.scsi_blk_len = blk_num * g_usbd_msc.scsi_blk_size;
622
623 if ((lba + blk_num) > g_usbd_msc.scsi_blk_nbr) {
624 USB_LOG_ERR("LBA out of range\r\n");
625 return false;
626 }
627
628 if (g_usbd_msc.cbw.dDataLength != g_usbd_msc.scsi_blk_len) {
629 return false;
630 }
631
632 g_usbd_msc.stage = MSC_DATA_OUT;
633 return true;
634 }
635 #endif
636
SCSI_processRead(void)637 static bool SCSI_processRead(void)
638 {
639 uint32_t transfer_len;
640
641 USB_LOG_DBG("read lba:%d\r\n", g_usbd_msc.start_sector);
642
643 transfer_len = MIN(g_usbd_msc.nsectors * g_usbd_msc.scsi_blk_size, CONFIG_USBDEV_MSC_BLOCK_SIZE);
644
645 if (usbd_msc_sector_read(g_usbd_msc.start_sector, g_usbd_msc.block_buffer, transfer_len) != 0) {
646 SCSI_SetSenseData(SCSI_KCQHE_UREINRESERVEDAREA);
647 return false;
648 }
649
650 usbd_ep_start_write(mass_ep_data[MSD_IN_EP_IDX].ep_addr, g_usbd_msc.block_buffer, transfer_len);
651
652 g_usbd_msc.start_sector += (transfer_len / g_usbd_msc.scsi_blk_size);
653 g_usbd_msc.nsectors -= (transfer_len / g_usbd_msc.scsi_blk_size);
654 g_usbd_msc.csw.dDataResidue -= transfer_len;
655
656 if (g_usbd_msc.nsectors == 0) {
657 g_usbd_msc.stage = MSC_SEND_CSW;
658 }
659
660 return true;
661 }
662
SCSI_processWrite(uint32_t nbytes)663 static bool SCSI_processWrite(uint32_t nbytes)
664 {
665 uint32_t data_len = 0;
666 USB_LOG_DBG("write lba:%d\r\n", g_usbd_msc.start_sector);
667
668 if (usbd_msc_sector_write(g_usbd_msc.start_sector, g_usbd_msc.block_buffer, nbytes) != 0) {
669 SCSI_SetSenseData(SCSI_KCQHE_WRITEFAULT);
670 return false;
671 }
672
673 g_usbd_msc.start_sector += (nbytes / g_usbd_msc.scsi_blk_size);
674 g_usbd_msc.nsectors -= (nbytes / g_usbd_msc.scsi_blk_size);
675 g_usbd_msc.csw.dDataResidue -= nbytes;
676
677 if (g_usbd_msc.nsectors == 0) {
678 usbd_msc_send_csw(CSW_STATUS_CMD_PASSED);
679 } else {
680 data_len = MIN(g_usbd_msc.nsectors * g_usbd_msc.scsi_blk_size, CONFIG_USBDEV_MSC_BLOCK_SIZE);
681 usbd_ep_start_read(mass_ep_data[MSD_OUT_EP_IDX].ep_addr, g_usbd_msc.block_buffer, data_len);
682 }
683
684 return true;
685 }
686
SCSI_CBWDecode(uint32_t nbytes)687 static bool SCSI_CBWDecode(uint32_t nbytes)
688 {
689 uint8_t *buf2send = g_usbd_msc.block_buffer;
690 uint32_t len2send = 0;
691 bool ret = false;
692
693 if (nbytes != sizeof(struct CBW)) {
694 USB_LOG_ERR("size != sizeof(cbw)\r\n");
695 SCSI_SetSenseData(SCSI_KCQIR_INVALIDCOMMAND);
696 return false;
697 }
698
699 g_usbd_msc.csw.dTag = g_usbd_msc.cbw.dTag;
700 g_usbd_msc.csw.dDataResidue = g_usbd_msc.cbw.dDataLength;
701
702 if ((g_usbd_msc.cbw.bLUN > 1) || (g_usbd_msc.cbw.dSignature != MSC_CBW_Signature) || (g_usbd_msc.cbw.bCBLength < 1) || (g_usbd_msc.cbw.bCBLength > 16)) {
703 SCSI_SetSenseData(SCSI_KCQIR_INVALIDCOMMAND);
704 return false;
705 } else {
706 USB_LOG_DBG("Decode CB:0x%02x\r\n", g_usbd_msc.cbw.CB[0]);
707 switch (g_usbd_msc.cbw.CB[0]) {
708 case SCSI_CMD_TESTUNITREADY:
709 ret = SCSI_testUnitReady(&buf2send, &len2send);
710 break;
711 case SCSI_CMD_REQUESTSENSE:
712 ret = SCSI_requestSense(&buf2send, &len2send);
713 break;
714 case SCSI_CMD_INQUIRY:
715 ret = SCSI_inquiry(&buf2send, &len2send);
716 break;
717 case SCSI_CMD_STARTSTOPUNIT:
718 ret = SCSI_startStopUnit(&buf2send, &len2send);
719 break;
720 case SCSI_CMD_PREVENTMEDIAREMOVAL:
721 ret = SCSI_preventAllowMediaRemoval(&buf2send, &len2send);
722 break;
723 case SCSI_CMD_MODESENSE6:
724 ret = SCSI_modeSense6(&buf2send, &len2send);
725 break;
726 case SCSI_CMD_MODESENSE10:
727 ret = SCSI_modeSense10(&buf2send, &len2send);
728 break;
729 case SCSI_CMD_READFORMATCAPACITIES:
730 ret = SCSI_readFormatCapacity(&buf2send, &len2send);
731 break;
732 case SCSI_CMD_READCAPACITY10:
733 ret = SCSI_readCapacity10(&buf2send, &len2send);
734 break;
735 case SCSI_CMD_READ10:
736 ret = SCSI_read10(NULL, 0);
737 break;
738 case SCSI_CMD_READ12:
739 ret = SCSI_read12(NULL, 0);
740 break;
741 case SCSI_CMD_WRITE10:
742 ret = SCSI_write10(NULL, 0);
743 break;
744 case SCSI_CMD_WRITE12:
745 ret = SCSI_write12(NULL, 0);
746 break;
747 case SCSI_CMD_VERIFY10:
748 //ret = SCSI_verify10(NULL, 0);
749 ret = false;
750 break;
751
752 default:
753 SCSI_SetSenseData(SCSI_KCQIR_INVALIDCOMMAND);
754 USB_LOG_WRN("unsupported cmd:0x%02x\r\n", g_usbd_msc.cbw.CB[0]);
755 ret = false;
756 break;
757 }
758 }
759 if (ret) {
760 if (g_usbd_msc.stage == MSC_READ_CBW) {
761 if (len2send) {
762 USB_LOG_DBG("Send info len:%d\r\n", len2send);
763 usbd_msc_send_info(buf2send, len2send);
764 } else {
765 usbd_msc_send_csw(CSW_STATUS_CMD_PASSED);
766 }
767 }
768 }
769 return ret;
770 }
771
mass_storage_bulk_out(uint8_t ep,uint32_t nbytes)772 void mass_storage_bulk_out(uint8_t ep, uint32_t nbytes)
773 {
774 switch (g_usbd_msc.stage) {
775 case MSC_READ_CBW:
776 if (SCSI_CBWDecode(nbytes) == false) {
777 USB_LOG_ERR("Command:0x%02x decode err\r\n", g_usbd_msc.cbw.CB[0]);
778 usbd_msc_bot_abort();
779 return;
780 }
781 break;
782 case MSC_DATA_OUT:
783 switch (g_usbd_msc.cbw.CB[0]) {
784 case SCSI_CMD_WRITE10:
785 case SCSI_CMD_WRITE12:
786 #ifdef CONFIG_USBDEV_MSC_THREAD
787 g_usbd_msc.nbytes = nbytes;
788 usb_osal_mq_send(g_usbd_msc.usbd_msc_mq, MSC_DATA_OUT);
789 #else
790 if (SCSI_processWrite(nbytes) == false) {
791 usbd_msc_send_csw(CSW_STATUS_CMD_FAILED); /* send fail status to host,and the host will retry*/
792 }
793 #endif
794 break;
795 default:
796 break;
797 }
798 break;
799 default:
800 break;
801 }
802 }
803
mass_storage_bulk_in(uint8_t ep,uint32_t nbytes)804 void mass_storage_bulk_in(uint8_t ep, uint32_t nbytes)
805 {
806 switch (g_usbd_msc.stage) {
807 case MSC_DATA_IN:
808 switch (g_usbd_msc.cbw.CB[0]) {
809 case SCSI_CMD_READ10:
810 case SCSI_CMD_READ12:
811 #ifdef CONFIG_USBDEV_MSC_THREAD
812 usb_osal_mq_send(g_usbd_msc.usbd_msc_mq, MSC_DATA_IN);
813 #else
814 if (SCSI_processRead() == false) {
815 usbd_msc_send_csw(CSW_STATUS_CMD_FAILED); /* send fail status to host,and the host will retry*/
816 return;
817 }
818 #endif
819 break;
820 default:
821 break;
822 }
823 break;
824 /*the device has to send a CSW*/
825 case MSC_SEND_CSW:
826 usbd_msc_send_csw(CSW_STATUS_CMD_PASSED);
827 break;
828
829 /*the host has received the CSW*/
830 case MSC_WAIT_CSW:
831 g_usbd_msc.stage = MSC_READ_CBW;
832 USB_LOG_DBG("Start reading cbw\r\n");
833 usbd_ep_start_read(mass_ep_data[MSD_OUT_EP_IDX].ep_addr, (uint8_t *)&g_usbd_msc.cbw, USB_SIZEOF_MSC_CBW);
834 break;
835
836 default:
837 break;
838 }
839 }
840
841 #ifdef CONFIG_USBDEV_MSC_THREAD
usbdev_msc_thread(void * argument)842 static void usbdev_msc_thread(void *argument)
843 {
844 uintptr_t event;
845 int ret;
846
847 while (1) {
848 ret = usb_osal_mq_recv(g_usbd_msc.usbd_msc_mq, (uintptr_t *)&event, USB_OSAL_WAITING_FOREVER);
849 if (ret < 0) {
850 continue;
851 }
852 USB_LOG_DBG("%d\r\n", event);
853 if (event == MSC_DATA_OUT) {
854 if (SCSI_processWrite(g_usbd_msc.nbytes) == false) {
855 usbd_msc_send_csw(CSW_STATUS_CMD_FAILED); /* send fail status to host,and the host will retry*/
856 }
857 } else if (event == MSC_DATA_IN) {
858 if (SCSI_processRead() == false) {
859 usbd_msc_send_csw(CSW_STATUS_CMD_FAILED); /* send fail status to host,and the host will retry*/
860 }
861 } else {
862 }
863 }
864 }
865 #endif
866
usbd_msc_init_intf(struct usbd_interface * intf,const uint8_t out_ep,const uint8_t in_ep)867 struct usbd_interface *usbd_msc_init_intf(struct usbd_interface *intf, const uint8_t out_ep, const uint8_t in_ep)
868 {
869 intf->class_interface_handler = msc_storage_class_interface_request_handler;
870 intf->class_endpoint_handler = NULL;
871 intf->vendor_handler = NULL;
872 intf->notify_handler = msc_storage_notify_handler;
873
874 mass_ep_data[MSD_OUT_EP_IDX].ep_addr = out_ep;
875 mass_ep_data[MSD_OUT_EP_IDX].ep_cb = mass_storage_bulk_out;
876 mass_ep_data[MSD_IN_EP_IDX].ep_addr = in_ep;
877 mass_ep_data[MSD_IN_EP_IDX].ep_cb = mass_storage_bulk_in;
878
879 usbd_add_endpoint(&mass_ep_data[MSD_OUT_EP_IDX]);
880 usbd_add_endpoint(&mass_ep_data[MSD_IN_EP_IDX]);
881
882 memset((uint8_t *)&g_usbd_msc, 0, sizeof(struct usbd_msc_priv));
883
884 usbd_msc_get_cap(0, &g_usbd_msc.scsi_blk_nbr, &g_usbd_msc.scsi_blk_size);
885
886 if (g_usbd_msc.scsi_blk_size > CONFIG_USBDEV_MSC_BLOCK_SIZE) {
887 USB_LOG_ERR("msc block buffer overflow\r\n");
888 return NULL;
889 }
890
891 #ifdef CONFIG_USBDEV_MSC_THREAD
892 g_usbd_msc.usbd_msc_mq = usb_osal_mq_create(1);
893 if (g_usbd_msc.usbd_msc_mq == NULL) {
894 return NULL;
895 }
896 g_usbd_msc.usbd_msc_thread = usb_osal_thread_create("usbd_msc", CONFIG_USBDEV_MSC_STACKSIZE, CONFIG_USBDEV_MSC_PRIO, usbdev_msc_thread, NULL);
897 if (g_usbd_msc.usbd_msc_thread == NULL) {
898 return NULL;
899 }
900 #endif
901 return intf;
902 }
903
usbd_msc_set_readonly(bool readonly)904 void usbd_msc_set_readonly(bool readonly)
905 {
906 g_usbd_msc.readonly = readonly;
907 }
908
usbd_msc_set_popup(void)909 bool usbd_msc_set_popup(void)
910 {
911 return g_usbd_msc.popup;
912 }
913