1 /*
2 * The Mass Storage protocol state machine in this file is based on mbed's
3 * implementation. We augment it by adding Zephyr's USB transport and Storage
4 * APIs.
5 *
6 * Copyright (c) 2010-2011 mbed.org, MIT License
7 * Copyright (c) 2016 Intel Corporation.
8 *
9 * Permission is hereby granted, free of charge, to any person obtaining a copy
10 * of this software and associated documentation files (the "Software"), to deal
11 * in the Software without restriction, including without limitation the rights
12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13 * copies of the Software, and to permit persons to whom the Software is
14 * furnished to do so, subject to the following conditions:
15 *
16 * The above copyright notice and this permission notice shall be included in
17 * all copies or substantial portions of the Software.
18 *
19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
25 * SOFTWARE.
26 */
27
28
29 /**
30 * @file
31 * @brief Mass Storage device class driver
32 *
33 * Driver for USB Mass Storage device class driver
34 */
35
36 #include <zephyr/init.h>
37 #include <errno.h>
38 #include <string.h>
39 #include <zephyr/sys/byteorder.h>
40 #include <zephyr/sys/__assert.h>
41 #include <zephyr/storage/disk_access.h>
42 #include <zephyr/usb/usb_device.h>
43 #include <usb_descriptor.h>
44
45 #include <zephyr/logging/log.h>
46 LOG_MODULE_REGISTER(usb_msc, CONFIG_USB_MASS_STORAGE_LOG_LEVEL);
47
48 /* MSC Subclass and Protocol Codes */
49 #define SCSI_TRANSPARENT_SUBCLASS 0x06
50 #define BULK_ONLY_TRANSPORT_PROTOCOL 0x50
51
52 /* MSC Request Codes for Bulk-Only Transport */
53 #define MSC_REQUEST_GET_MAX_LUN 0xFE
54 #define MSC_REQUEST_RESET 0xFF
55
56 /* MSC Command Block Wrapper (CBW) Signature */
57 #define CBW_Signature 0x43425355
58
59 /* MSC Command Block Wrapper Flags */
60 #define CBW_DIRECTION_DATA_IN 0x80
61
62 /* MSC Bulk-Only Command Block Wrapper (CBW) */
63 struct CBW {
64 uint32_t Signature;
65 uint32_t Tag;
66 uint32_t DataLength;
67 uint8_t Flags;
68 uint8_t LUN;
69 uint8_t CBLength;
70 uint8_t CB[16];
71 } __packed;
72
73 /* MSC Command Status Wrapper (CBW) Signature */
74 #define CSW_Signature 0x53425355
75
76 /* MSC Command Block Status Values */
77 #define CSW_STATUS_CMD_PASSED 0x00
78 #define CSW_STATUS_CMD_FAILED 0x01
79 #define CSW_STATUS_PHASE_ERROR 0x02
80
81 /* MSC Bulk-Only Command Status Wrapper (CSW) */
82 struct CSW {
83 uint32_t Signature;
84 uint32_t Tag;
85 uint32_t DataResidue;
86 uint8_t Status;
87 } __packed;
88
89 /* SCSI transparent command set used by MSC */
90 #define TEST_UNIT_READY 0x00
91 #define REQUEST_SENSE 0x03
92 #define FORMAT_UNIT 0x04
93 #define INQUIRY 0x12
94 #define MODE_SELECT6 0x15
95 #define MODE_SENSE6 0x1A
96 #define START_STOP_UNIT 0x1B
97 #define MEDIA_REMOVAL 0x1E
98 #define READ_FORMAT_CAPACITIES 0x23
99 #define READ_CAPACITY 0x25
100 #define READ10 0x28
101 #define WRITE10 0x2A
102 #define VERIFY10 0x2F
103 #define READ12 0xA8
104 #define WRITE12 0xAA
105 #define MODE_SELECT10 0x55
106 #define MODE_SENSE10 0x5A
107
108 /* max USB packet size */
109 #define MAX_PACKET CONFIG_MASS_STORAGE_BULK_EP_MPS
110
111 #define BLOCK_SIZE 512
112 #define DISK_THREAD_PRIO -5
113
114 BUILD_ASSERT(MAX_PACKET <= BLOCK_SIZE);
115
116 #define THREAD_OP_READ_QUEUED 1
117 #define THREAD_OP_WRITE_QUEUED 3
118 #define THREAD_OP_WRITE_DONE 4
119
120 #define MASS_STORAGE_IN_EP_ADDR 0x82
121 #define MASS_STORAGE_OUT_EP_ADDR 0x01
122
123 struct usb_mass_config {
124 struct usb_if_descriptor if0;
125 struct usb_ep_descriptor if0_in_ep;
126 struct usb_ep_descriptor if0_out_ep;
127 } __packed;
128
129 USBD_CLASS_DESCR_DEFINE(primary, 0) struct usb_mass_config mass_cfg = {
130 /* Interface descriptor */
131 .if0 = {
132 .bLength = sizeof(struct usb_if_descriptor),
133 .bDescriptorType = USB_DESC_INTERFACE,
134 .bInterfaceNumber = 0,
135 .bAlternateSetting = 0,
136 .bNumEndpoints = 2,
137 .bInterfaceClass = USB_BCC_MASS_STORAGE,
138 .bInterfaceSubClass = SCSI_TRANSPARENT_SUBCLASS,
139 .bInterfaceProtocol = BULK_ONLY_TRANSPORT_PROTOCOL,
140 .iInterface = 0,
141 },
142 /* First Endpoint IN */
143 .if0_in_ep = {
144 .bLength = sizeof(struct usb_ep_descriptor),
145 .bDescriptorType = USB_DESC_ENDPOINT,
146 .bEndpointAddress = MASS_STORAGE_IN_EP_ADDR,
147 .bmAttributes = USB_DC_EP_BULK,
148 .wMaxPacketSize =
149 sys_cpu_to_le16(CONFIG_MASS_STORAGE_BULK_EP_MPS),
150 .bInterval = 0x00,
151 },
152 /* Second Endpoint OUT */
153 .if0_out_ep = {
154 .bLength = sizeof(struct usb_ep_descriptor),
155 .bDescriptorType = USB_DESC_ENDPOINT,
156 .bEndpointAddress = MASS_STORAGE_OUT_EP_ADDR,
157 .bmAttributes = USB_DC_EP_BULK,
158 .wMaxPacketSize =
159 sys_cpu_to_le16(CONFIG_MASS_STORAGE_BULK_EP_MPS),
160 .bInterval = 0x00,
161 },
162 };
163
164 static volatile int thread_op;
165 static K_KERNEL_STACK_DEFINE(mass_thread_stack, CONFIG_MASS_STORAGE_STACK_SIZE);
166 static struct k_thread mass_thread_data;
167 static struct k_sem disk_wait_sem;
168 static volatile uint32_t defered_wr_sz;
169
170 /*
171 * Keep block buffer larger than BLOCK_SIZE for the case
172 * the dCBWDataTransferLength is multiple of the BLOCK_SIZE and
173 * the length of the transferred data is not aligned to the BLOCK_SIZE.
174 *
175 * Align for cases where the underlying disk access requires word-aligned
176 * addresses.
177 */
178 static uint8_t __aligned(4) page[BLOCK_SIZE + CONFIG_MASS_STORAGE_BULK_EP_MPS];
179
180 /* Initialized during mass_storage_init() */
181 static uint32_t block_count;
182 static const char *disk_pdrv = CONFIG_MASS_STORAGE_DISK_NAME;
183
184 #define MSD_OUT_EP_IDX 0
185 #define MSD_IN_EP_IDX 1
186
187 static void mass_storage_bulk_out(uint8_t ep,
188 enum usb_dc_ep_cb_status_code ep_status);
189 static void mass_storage_bulk_in(uint8_t ep,
190 enum usb_dc_ep_cb_status_code ep_status);
191
192 /* Describe EndPoints configuration */
193 static struct usb_ep_cfg_data mass_ep_data[] = {
194 {
195 .ep_cb = mass_storage_bulk_out,
196 .ep_addr = MASS_STORAGE_OUT_EP_ADDR
197 },
198 {
199 .ep_cb = mass_storage_bulk_in,
200 .ep_addr = MASS_STORAGE_IN_EP_ADDR
201 }
202 };
203
204 /* CSW Status */
205 enum Status {
206 CSW_PASSED,
207 CSW_FAILED,
208 CSW_ERROR,
209 };
210
211 /* MSC Bulk-only Stage */
212 enum Stage {
213 MSC_READ_CBW, /* wait a CBW */
214 MSC_ERROR, /* error */
215 MSC_PROCESS_CBW, /* process a CBW request */
216 MSC_SEND_CSW, /* send a CSW */
217 MSC_WAIT_CSW /* wait that a CSW has been effectively sent */
218 };
219
220 /* state of the bulk-only state machine */
221 static enum Stage stage;
222
223 /*current CBW*/
224 static struct CBW cbw;
225
226 /*CSW which will be sent*/
227 static struct CSW csw;
228
229 /*addr where will be read or written data*/
230 static uint32_t curr_lba;
231
232 /*length of a reading or writing*/
233 static uint32_t length;
234
235 /*offset into curr_lba for read/write*/
236 static uint16_t curr_offset;
237
238 static uint8_t max_lun_count;
239
240 /*memory OK (after a memoryVerify)*/
241 static bool memOK;
242
243 #define INQ_VENDOR_ID_LEN 8
244 #define INQ_PRODUCT_ID_LEN 16
245 #define INQ_REVISION_LEN 4
246
247 struct dabc_inquiry_data {
248 uint8_t head[8];
249 uint8_t t10_vid[INQ_VENDOR_ID_LEN];
250 uint8_t product_id[INQ_PRODUCT_ID_LEN];
251 uint8_t product_rev[INQ_REVISION_LEN];
252 } __packed;
253
254 static const struct dabc_inquiry_data inq_rsp = {
255 .head = {0x00, 0x80, 0x00, 0x01, 36 - 4, 0x80, 0x00, 0x00},
256 .t10_vid = CONFIG_MASS_STORAGE_INQ_VENDOR_ID,
257 .product_id = CONFIG_MASS_STORAGE_INQ_PRODUCT_ID,
258 .product_rev = CONFIG_MASS_STORAGE_INQ_REVISION,
259 };
260
261 BUILD_ASSERT(sizeof(CONFIG_MASS_STORAGE_INQ_VENDOR_ID) == (INQ_VENDOR_ID_LEN + 1),
262 "CONFIG_MASS_STORAGE_INQ_VENDOR_ID must be 8 characters (pad with spaces)");
263
264 BUILD_ASSERT(sizeof(CONFIG_MASS_STORAGE_INQ_PRODUCT_ID) == (INQ_PRODUCT_ID_LEN + 1),
265 "CONFIG_MASS_STORAGE_INQ_PRODUCT_ID must be 16 characters (pad with spaces)");
266
267 BUILD_ASSERT(sizeof(CONFIG_MASS_STORAGE_INQ_REVISION) == (INQ_REVISION_LEN + 1),
268 "CONFIG_MASS_STORAGE_INQ_REVISION must be 4 characters (pad with spaces)");
269
msd_state_machine_reset(void)270 static void msd_state_machine_reset(void)
271 {
272 stage = MSC_READ_CBW;
273 }
274
msd_init(void)275 static void msd_init(void)
276 {
277 (void)memset((void *)&cbw, 0, sizeof(struct CBW));
278 (void)memset((void *)&csw, 0, sizeof(struct CSW));
279 (void)memset(page, 0, sizeof(page));
280 curr_lba = 0U;
281 length = 0U;
282 curr_offset = 0U;
283 }
284
sendCSW(void)285 static void sendCSW(void)
286 {
287 csw.Signature = CSW_Signature;
288 if (usb_write(mass_ep_data[MSD_IN_EP_IDX].ep_addr, (uint8_t *)&csw,
289 sizeof(struct CSW), NULL) != 0) {
290 LOG_ERR("usb write failure");
291 }
292 stage = MSC_WAIT_CSW;
293 }
294
fail(void)295 static void fail(void)
296 {
297 if (cbw.DataLength) {
298 /* Stall data stage */
299 if ((cbw.Flags & 0x80) != 0U) {
300 usb_ep_set_stall(mass_ep_data[MSD_IN_EP_IDX].ep_addr);
301 } else {
302 usb_ep_set_stall(mass_ep_data[MSD_OUT_EP_IDX].ep_addr);
303 }
304 }
305
306 csw.Status = CSW_FAILED;
307 sendCSW();
308 }
309
write(uint8_t * buf,uint16_t size)310 static bool write(uint8_t *buf, uint16_t size)
311 {
312 if (size >= cbw.DataLength) {
313 size = cbw.DataLength;
314 }
315
316 /* updating the State Machine , so that we send CSW when this
317 * transfer is complete, ie when we get a bulk in callback
318 */
319 stage = MSC_SEND_CSW;
320
321 if (usb_write(mass_ep_data[MSD_IN_EP_IDX].ep_addr, buf, size, NULL)) {
322 LOG_ERR("USB write failed");
323 return false;
324 }
325
326 csw.DataResidue -= size;
327 csw.Status = CSW_PASSED;
328 return true;
329 }
330
331 /**
332 * @brief Handler called for Class requests not handled by the USB stack.
333 *
334 * @param pSetup Information about the request to execute.
335 * @param len Size of the buffer.
336 * @param data Buffer containing the request result.
337 *
338 * @return 0 on success, negative errno code on fail.
339 */
mass_storage_class_handle_req(struct usb_setup_packet * setup,int32_t * len,uint8_t ** data)340 static int mass_storage_class_handle_req(struct usb_setup_packet *setup,
341 int32_t *len, uint8_t **data)
342 {
343 if (setup->wIndex != mass_cfg.if0.bInterfaceNumber ||
344 setup->wValue != 0) {
345 LOG_ERR("Invalid setup parameters");
346 return -EINVAL;
347 }
348
349 if (usb_reqtype_is_to_device(setup)) {
350 if (setup->bRequest == MSC_REQUEST_RESET &&
351 setup->wLength == 0) {
352 LOG_DBG("MSC_REQUEST_RESET");
353 msd_state_machine_reset();
354 return 0;
355 }
356 } else {
357 if (setup->bRequest == MSC_REQUEST_GET_MAX_LUN &&
358 setup->wLength == 1) {
359 LOG_DBG("MSC_REQUEST_GET_MAX_LUN");
360 max_lun_count = 0U;
361 *data = (uint8_t *)(&max_lun_count);
362 *len = 1;
363 return 0;
364 }
365 }
366
367 LOG_WRN("Unsupported bmRequestType 0x%02x bRequest 0x%02x",
368 setup->bmRequestType, setup->bRequest);
369 return -ENOTSUP;
370 }
371
testUnitReady(void)372 static void testUnitReady(void)
373 {
374 if (cbw.DataLength != 0U) {
375 if ((cbw.Flags & 0x80) != 0U) {
376 LOG_WRN("Stall IN endpoint");
377 usb_ep_set_stall(mass_ep_data[MSD_IN_EP_IDX].ep_addr);
378 } else {
379 LOG_WRN("Stall OUT endpoint");
380 usb_ep_set_stall(mass_ep_data[MSD_OUT_EP_IDX].ep_addr);
381 }
382 }
383
384 csw.Status = CSW_PASSED;
385 sendCSW();
386 }
387
requestSense(void)388 static bool requestSense(void)
389 {
390 uint8_t request_sense[] = {
391 0x70,
392 0x00,
393 0x05, /* Sense Key: illegal request */
394 0x00,
395 0x00,
396 0x00,
397 0x00,
398 0x0A,
399 0x00,
400 0x00,
401 0x00,
402 0x00,
403 0x30,
404 0x01,
405 0x00,
406 0x00,
407 0x00,
408 0x00,
409 };
410
411 return write(request_sense, sizeof(request_sense));
412 }
413
inquiryRequest(void)414 static bool inquiryRequest(void)
415 {
416 return write((uint8_t *)&inq_rsp, sizeof(inq_rsp));
417 }
418
modeSense6(void)419 static bool modeSense6(void)
420 {
421 uint8_t sense6[] = { 0x03, 0x00, 0x00, 0x00 };
422
423 return write(sense6, sizeof(sense6));
424 }
425
readFormatCapacity(void)426 static bool readFormatCapacity(void)
427 {
428 uint8_t capacity[] = { 0x00, 0x00, 0x00, 0x08,
429 (uint8_t)((block_count >> 24) & 0xff),
430 (uint8_t)((block_count >> 16) & 0xff),
431 (uint8_t)((block_count >> 8) & 0xff),
432 (uint8_t)((block_count >> 0) & 0xff),
433
434 0x02,
435 (uint8_t)((BLOCK_SIZE >> 16) & 0xff),
436 (uint8_t)((BLOCK_SIZE >> 8) & 0xff),
437 (uint8_t)((BLOCK_SIZE >> 0) & 0xff),
438 };
439
440 return write(capacity, sizeof(capacity));
441 }
442
readCapacity(void)443 static bool readCapacity(void)
444 {
445 uint8_t capacity[8];
446
447 sys_put_be32(block_count - 1, &capacity[0]);
448 sys_put_be32(BLOCK_SIZE, &capacity[4]);
449
450 return write(capacity, sizeof(capacity));
451 }
452
thread_memory_read_done(void)453 static void thread_memory_read_done(void)
454 {
455 uint32_t n = length;
456
457 if (n > MAX_PACKET) {
458 n = MAX_PACKET;
459 }
460 if (n > BLOCK_SIZE - curr_offset) {
461 n = BLOCK_SIZE - curr_offset;
462 }
463
464 if (usb_write(mass_ep_data[MSD_IN_EP_IDX].ep_addr,
465 &page[curr_offset], n, NULL) != 0) {
466 LOG_ERR("Failed to write EP 0x%x",
467 mass_ep_data[MSD_IN_EP_IDX].ep_addr);
468 }
469 curr_offset += n;
470 if (curr_offset >= BLOCK_SIZE) {
471 curr_offset -= BLOCK_SIZE;
472 curr_lba += 1;
473 }
474 length -= n;
475 csw.DataResidue -= n;
476
477 if (!length || (stage != MSC_PROCESS_CBW)) {
478 csw.Status = (stage == MSC_PROCESS_CBW) ?
479 CSW_PASSED : CSW_FAILED;
480 stage = (stage == MSC_PROCESS_CBW) ? MSC_SEND_CSW : stage;
481 }
482 }
483
484
memoryRead(void)485 static void memoryRead(void)
486 {
487 if (curr_lba >= block_count) {
488 LOG_WRN("Attempt to read past end of device: lba=%u", curr_lba);
489 fail();
490 return;
491 }
492
493 if (!curr_offset) {
494 /* we need a new block */
495 thread_op = THREAD_OP_READ_QUEUED;
496 LOG_DBG("Signal thread for %u", curr_lba);
497 k_sem_give(&disk_wait_sem);
498 } else {
499 thread_memory_read_done();
500 }
501 }
502
check_cbw_data_length(void)503 static bool check_cbw_data_length(void)
504 {
505 if (!cbw.DataLength) {
506 LOG_WRN("Zero length in CBW");
507 csw.Status = CSW_FAILED;
508 sendCSW();
509 return false;
510 }
511
512 return true;
513 }
514
infoTransfer(void)515 static bool infoTransfer(void)
516 {
517 uint32_t n;
518
519 if (!check_cbw_data_length()) {
520 return false;
521 }
522
523 /* Logical Block Address of First Block */
524 n = sys_get_be32(&cbw.CB[2]);
525
526 LOG_DBG("LBA (block) : 0x%x ", n);
527 if (n >= block_count) {
528 LOG_ERR("LBA out of range");
529 fail();
530 return false;
531 }
532
533 curr_lba = n;
534 curr_offset = 0U;
535
536 /* Number of Blocks to transfer */
537 switch (cbw.CB[0]) {
538 case READ10:
539 case WRITE10:
540 case VERIFY10:
541 n = sys_get_be16(&cbw.CB[7]);
542 break;
543
544 case READ12:
545 case WRITE12:
546 n = sys_get_be32(&cbw.CB[6]);
547 break;
548 }
549
550 LOG_DBG("Size (block) : 0x%x ", n);
551 length = n * BLOCK_SIZE;
552
553 if (cbw.DataLength != length) {
554 LOG_ERR("DataLength mismatch");
555 fail();
556 return false;
557 }
558
559 return true;
560 }
561
CBWDecode(uint8_t * buf,uint16_t size)562 static void CBWDecode(uint8_t *buf, uint16_t size)
563 {
564 if (size != sizeof(cbw)) {
565 LOG_ERR("size != sizeof(cbw)");
566 return;
567 }
568
569 memcpy((uint8_t *)&cbw, buf, size);
570 if (cbw.Signature != CBW_Signature) {
571 LOG_ERR("CBW Signature Mismatch");
572 return;
573 }
574
575 csw.Tag = cbw.Tag;
576 csw.DataResidue = cbw.DataLength;
577
578 if ((cbw.CBLength < 1) || (cbw.CBLength > 16) || (cbw.LUN != 0U)) {
579 LOG_WRN("cbw.CBLength %d", cbw.CBLength);
580 fail();
581 } else {
582 switch (cbw.CB[0]) {
583 case TEST_UNIT_READY:
584 LOG_DBG(">> TUR");
585 testUnitReady();
586 break;
587 case REQUEST_SENSE:
588 LOG_DBG(">> REQ_SENSE");
589 if (check_cbw_data_length()) {
590 requestSense();
591 }
592 break;
593 case INQUIRY:
594 LOG_DBG(">> INQ");
595 if (check_cbw_data_length()) {
596 inquiryRequest();
597 }
598 break;
599 case MODE_SENSE6:
600 LOG_DBG(">> MODE_SENSE6");
601 if (check_cbw_data_length()) {
602 modeSense6();
603 }
604 break;
605 case READ_FORMAT_CAPACITIES:
606 LOG_DBG(">> READ_FORMAT_CAPACITY");
607 if (check_cbw_data_length()) {
608 readFormatCapacity();
609 }
610 break;
611 case READ_CAPACITY:
612 LOG_DBG(">> READ_CAPACITY");
613 if (check_cbw_data_length()) {
614 readCapacity();
615 }
616 break;
617 case READ10:
618 case READ12:
619 LOG_DBG(">> READ");
620 if (infoTransfer()) {
621 if ((cbw.Flags & 0x80)) {
622 stage = MSC_PROCESS_CBW;
623 memoryRead();
624 } else {
625 usb_ep_set_stall(
626 mass_ep_data[MSD_OUT_EP_IDX].ep_addr);
627 LOG_WRN("Stall OUT endpoint");
628 csw.Status = CSW_ERROR;
629 sendCSW();
630 }
631 }
632 break;
633 case WRITE10:
634 case WRITE12:
635 LOG_DBG(">> WRITE");
636 if (infoTransfer()) {
637 if (!(cbw.Flags & 0x80)) {
638 stage = MSC_PROCESS_CBW;
639 } else {
640 usb_ep_set_stall(
641 mass_ep_data[MSD_IN_EP_IDX].ep_addr);
642 LOG_WRN("Stall IN endpoint");
643 csw.Status = CSW_ERROR;
644 sendCSW();
645 }
646 }
647 break;
648 case VERIFY10:
649 LOG_DBG(">> VERIFY10");
650 if (!(cbw.CB[1] & 0x02)) {
651 csw.Status = CSW_PASSED;
652 sendCSW();
653 break;
654 }
655 if (infoTransfer()) {
656 if (!(cbw.Flags & 0x80)) {
657 stage = MSC_PROCESS_CBW;
658 memOK = true;
659 } else {
660 usb_ep_set_stall(
661 mass_ep_data[MSD_IN_EP_IDX].ep_addr);
662 LOG_WRN("Stall IN endpoint");
663 csw.Status = CSW_ERROR;
664 sendCSW();
665 }
666 }
667 break;
668 case MEDIA_REMOVAL:
669 LOG_DBG(">> MEDIA_REMOVAL");
670 csw.Status = CSW_PASSED;
671 sendCSW();
672 break;
673 default:
674 LOG_WRN(">> default CB[0] %x", cbw.CB[0]);
675 fail();
676 break;
677 } /*switch(cbw.CB[0])*/
678 } /* else */
679
680 }
681
memoryVerify(uint8_t * buf,uint16_t size)682 static void memoryVerify(uint8_t *buf, uint16_t size)
683 {
684 uint32_t n;
685
686 if (curr_lba >= block_count) {
687 LOG_WRN("Attempt to read past end of device: lba=%u", curr_lba);
688 fail();
689 return;
690 }
691
692 /* BUG: if a packet crosses block boundaries, we will probably fail. */
693
694 /* beginning of a new block -> load a whole block in RAM */
695 if (!curr_offset) {
696 LOG_DBG("Disk READ sector %u", curr_lba);
697 if (disk_access_read(disk_pdrv, page, curr_lba, 1)) {
698 LOG_ERR("---- Disk Read Error %u", curr_lba);
699 }
700 }
701
702 /* info are in RAM -> no need to re-read memory */
703 for (n = 0U; n < size; n++) {
704 if (page[curr_offset + n] != buf[n]) {
705 LOG_DBG("Mismatch sector %u offset %u",
706 curr_lba, curr_offset + n);
707 memOK = false;
708 break;
709 }
710 }
711
712 curr_offset += n;
713 if (curr_offset >= BLOCK_SIZE) {
714 curr_offset -= BLOCK_SIZE;
715 curr_lba += 1;
716 }
717 length -= size;
718 csw.DataResidue -= size;
719
720 if (!length || (stage != MSC_PROCESS_CBW)) {
721 csw.Status = (memOK && (stage == MSC_PROCESS_CBW)) ?
722 CSW_PASSED : CSW_FAILED;
723 sendCSW();
724 }
725 }
726
memoryWrite(uint8_t * buf,uint16_t size)727 static void memoryWrite(uint8_t *buf, uint16_t size)
728 {
729 if (curr_lba >= block_count) {
730 LOG_WRN("Attempt to write past end of device: lba=%u", curr_lba);
731 fail();
732 return;
733 }
734
735 /* we fill an array in RAM of 1 block before writing it in memory */
736 for (int i = 0; i < size; i++) {
737 page[curr_offset + i] = buf[i];
738 }
739
740 /* if the array is filled, write it in memory */
741 if (curr_offset + size >= BLOCK_SIZE) {
742 if (!(disk_access_status(disk_pdrv) &
743 DISK_STATUS_WR_PROTECT)) {
744 LOG_DBG("Disk WRITE Qd %u", curr_lba);
745 thread_op = THREAD_OP_WRITE_QUEUED; /* write_queued */
746 defered_wr_sz = size;
747 k_sem_give(&disk_wait_sem);
748 return;
749 }
750 }
751
752 curr_offset += size;
753 length -= size;
754 csw.DataResidue -= size;
755
756 if ((!length) || (stage != MSC_PROCESS_CBW)) {
757 csw.Status = (stage == MSC_ERROR) ? CSW_FAILED : CSW_PASSED;
758 sendCSW();
759 }
760 }
761
762
mass_storage_bulk_out(uint8_t ep,enum usb_dc_ep_cb_status_code ep_status)763 static void mass_storage_bulk_out(uint8_t ep,
764 enum usb_dc_ep_cb_status_code ep_status)
765 {
766 uint32_t bytes_read = 0U;
767 uint8_t bo_buf[CONFIG_MASS_STORAGE_BULK_EP_MPS];
768
769 ARG_UNUSED(ep_status);
770
771 usb_ep_read_wait(ep, bo_buf, CONFIG_MASS_STORAGE_BULK_EP_MPS,
772 &bytes_read);
773
774 switch (stage) {
775 /*the device has to decode the CBW received*/
776 case MSC_READ_CBW:
777 LOG_DBG("> BO - MSC_READ_CBW");
778 CBWDecode(bo_buf, bytes_read);
779 break;
780
781 /*the device has to receive data from the host*/
782 case MSC_PROCESS_CBW:
783 switch (cbw.CB[0]) {
784 case WRITE10:
785 case WRITE12:
786 /* LOG_DBG("> BO - PROC_CBW WR");*/
787 memoryWrite(bo_buf, bytes_read);
788 break;
789 case VERIFY10:
790 LOG_DBG("> BO - PROC_CBW VER");
791 memoryVerify(bo_buf, bytes_read);
792 break;
793 default:
794 LOG_ERR("> BO - PROC_CBW default <<ERROR!!!>>");
795 break;
796 }
797 break;
798
799 /*an error has occurred: stall endpoint and send CSW*/
800 default:
801 LOG_WRN("Stall OUT endpoint, stage: %d", stage);
802 usb_ep_set_stall(ep);
803 csw.Status = CSW_ERROR;
804 sendCSW();
805 break;
806 }
807
808 if (thread_op != THREAD_OP_WRITE_QUEUED) {
809 usb_ep_read_continue(ep);
810 } else {
811 LOG_DBG("> BO not clearing NAKs yet");
812 }
813
814 }
815
thread_memory_write_done(void)816 static void thread_memory_write_done(void)
817 {
818 uint32_t size = defered_wr_sz;
819 size_t overflowed_len = (curr_offset + size) - BLOCK_SIZE;
820
821 if (BLOCK_SIZE > (curr_offset + size)) {
822 overflowed_len = 0;
823 }
824
825 if (overflowed_len > 0) {
826 memmove(page, &page[BLOCK_SIZE], overflowed_len);
827 }
828
829 curr_offset = overflowed_len;
830 curr_lba += 1;
831 length -= size;
832 csw.DataResidue -= size;
833
834 if (!length) {
835 if (disk_access_ioctl(disk_pdrv, DISK_IOCTL_CTRL_SYNC, NULL)) {
836 LOG_ERR("!! Disk cache sync error !!");
837 }
838 }
839
840 if ((!length) || (stage != MSC_PROCESS_CBW)) {
841 csw.Status = (stage == MSC_ERROR) ? CSW_FAILED : CSW_PASSED;
842 sendCSW();
843 }
844
845 thread_op = THREAD_OP_WRITE_DONE;
846
847 usb_ep_read_continue(mass_ep_data[MSD_OUT_EP_IDX].ep_addr);
848 }
849
850 /**
851 * @brief EP Bulk IN handler, used to send data to the Host
852 *
853 * @param ep Endpoint address.
854 * @param ep_status Endpoint status code.
855 *
856 * @return N/A.
857 */
mass_storage_bulk_in(uint8_t ep,enum usb_dc_ep_cb_status_code ep_status)858 static void mass_storage_bulk_in(uint8_t ep,
859 enum usb_dc_ep_cb_status_code ep_status)
860 {
861 ARG_UNUSED(ep_status);
862 ARG_UNUSED(ep);
863
864 switch (stage) {
865 /*the device has to send data to the host*/
866 case MSC_PROCESS_CBW:
867 switch (cbw.CB[0]) {
868 case READ10:
869 case READ12:
870 /* LOG_DBG("< BI - PROC_CBW READ"); */
871 memoryRead();
872 break;
873 default:
874 LOG_ERR("< BI-PROC_CBW default <<ERROR!!>>");
875 break;
876 }
877 break;
878
879 /*the device has to send a CSW*/
880 case MSC_SEND_CSW:
881 LOG_DBG("< BI - MSC_SEND_CSW");
882 sendCSW();
883 break;
884
885 /*the host has received the CSW -> we wait a CBW*/
886 case MSC_WAIT_CSW:
887 LOG_DBG("< BI - MSC_WAIT_CSW");
888 stage = MSC_READ_CBW;
889 break;
890
891 /*an error has occurred*/
892 default:
893 LOG_WRN("Stall IN endpoint, stage: %d", stage);
894 usb_ep_set_stall(mass_ep_data[MSD_IN_EP_IDX].ep_addr);
895 sendCSW();
896 break;
897 }
898 }
899
900
901
902 /**
903 * @brief Callback used to know the USB connection status
904 *
905 * @param status USB device status code.
906 *
907 * @return N/A.
908 */
mass_storage_status_cb(struct usb_cfg_data * cfg,enum usb_dc_status_code status,const uint8_t * param)909 static void mass_storage_status_cb(struct usb_cfg_data *cfg,
910 enum usb_dc_status_code status,
911 const uint8_t *param)
912 {
913 ARG_UNUSED(param);
914 ARG_UNUSED(cfg);
915
916 /* Check the USB status and do needed action if required */
917 switch (status) {
918 case USB_DC_ERROR:
919 LOG_DBG("USB device error");
920 break;
921 case USB_DC_RESET:
922 LOG_DBG("USB device reset detected");
923 msd_state_machine_reset();
924 msd_init();
925 break;
926 case USB_DC_CONNECTED:
927 LOG_DBG("USB device connected");
928 break;
929 case USB_DC_CONFIGURED:
930 LOG_DBG("USB device configured");
931 break;
932 case USB_DC_DISCONNECTED:
933 LOG_DBG("USB device disconnected");
934 break;
935 case USB_DC_SUSPEND:
936 LOG_DBG("USB device suspended");
937 break;
938 case USB_DC_RESUME:
939 LOG_DBG("USB device resumed");
940 break;
941 case USB_DC_INTERFACE:
942 LOG_DBG("USB interface selected");
943 break;
944 case USB_DC_SOF:
945 break;
946 case USB_DC_UNKNOWN:
947 default:
948 LOG_DBG("USB unknown state");
949 break;
950 }
951 }
952
mass_interface_config(struct usb_desc_header * head,uint8_t bInterfaceNumber)953 static void mass_interface_config(struct usb_desc_header *head,
954 uint8_t bInterfaceNumber)
955 {
956 ARG_UNUSED(head);
957
958 mass_cfg.if0.bInterfaceNumber = bInterfaceNumber;
959 }
960
961 /* Configuration of the Mass Storage Device send to the USB Driver */
962 USBD_DEFINE_CFG_DATA(mass_storage_config) = {
963 .usb_device_description = NULL,
964 .interface_config = mass_interface_config,
965 .interface_descriptor = &mass_cfg.if0,
966 .cb_usb_status = mass_storage_status_cb,
967 .interface = {
968 .class_handler = mass_storage_class_handle_req,
969 .custom_handler = NULL,
970 },
971 .num_endpoints = ARRAY_SIZE(mass_ep_data),
972 .endpoint = mass_ep_data
973 };
974
mass_thread_main(void * p1,void * p2,void * p3)975 static void mass_thread_main(void *p1, void *p2, void *p3)
976 {
977 ARG_UNUSED(p1);
978 ARG_UNUSED(p2);
979 ARG_UNUSED(p3);
980
981 while (1) {
982 k_sem_take(&disk_wait_sem, K_FOREVER);
983 LOG_DBG("sem %d", thread_op);
984
985 switch (thread_op) {
986 case THREAD_OP_READ_QUEUED:
987 if (disk_access_read(disk_pdrv,
988 page, curr_lba, 1)) {
989 LOG_ERR("!! Disk Read Error %u !",
990 curr_lba);
991 }
992
993 thread_memory_read_done();
994 break;
995 case THREAD_OP_WRITE_QUEUED:
996 if (disk_access_write(disk_pdrv,
997 page, curr_lba, 1)) {
998 LOG_ERR("!!!!! Disk Write Error %u !!!!!",
999 curr_lba);
1000 }
1001 thread_memory_write_done();
1002 break;
1003 default:
1004 LOG_ERR("XXXXXX thread_op %d ! XXXXX", thread_op);
1005 }
1006 }
1007 }
1008
1009 /**
1010 * @brief Initialize USB mass storage setup
1011 *
1012 * This routine is called to reset the USB device controller chip to a
1013 * quiescent state. Also it initializes the backing storage and initializes
1014 * the mass storage protocol state.
1015 *
1016 * @param dev device struct.
1017 *
1018 * @return negative errno code on fatal failure, 0 otherwise
1019 */
mass_storage_init(void)1020 static int mass_storage_init(void)
1021 {
1022 uint32_t block_size = 0U;
1023
1024
1025 if (disk_access_init(disk_pdrv) != 0) {
1026 LOG_ERR("Storage init ERROR !!!! - Aborting USB init");
1027 return 0;
1028 }
1029
1030 if (disk_access_ioctl(disk_pdrv,
1031 DISK_IOCTL_GET_SECTOR_COUNT, &block_count)) {
1032 LOG_ERR("Unable to get sector count - Aborting USB init");
1033 return 0;
1034 }
1035
1036 if (disk_access_ioctl(disk_pdrv,
1037 DISK_IOCTL_GET_SECTOR_SIZE, &block_size)) {
1038 LOG_ERR("Unable to get sector size - Aborting USB init");
1039 return 0;
1040 }
1041
1042 if (block_size != BLOCK_SIZE) {
1043 LOG_ERR("Block Size reported by the storage side is "
1044 "different from Mass Storage Class page Buffer - "
1045 "Aborting");
1046 return 0;
1047 }
1048
1049
1050 LOG_INF("Sect Count %u", block_count);
1051 LOG_INF("Memory Size %llu", (uint64_t) block_count * BLOCK_SIZE);
1052
1053 msd_state_machine_reset();
1054 msd_init();
1055
1056 k_sem_init(&disk_wait_sem, 0, 1);
1057
1058 /* Start a thread to offload disk ops */
1059 k_thread_create(&mass_thread_data, mass_thread_stack,
1060 CONFIG_MASS_STORAGE_STACK_SIZE,
1061 mass_thread_main, NULL, NULL, NULL,
1062 DISK_THREAD_PRIO, 0, K_NO_WAIT);
1063
1064 k_thread_name_set(&mass_thread_data, "usb_mass");
1065
1066 return 0;
1067 }
1068
1069 SYS_INIT(mass_storage_init, APPLICATION, CONFIG_KERNEL_INIT_PRIORITY_DEVICE);
1070
