1 /*
2  * Copyright (c) 2022, sakumisu
3  *
4  * SPDX-License-Identifier: Apache-2.0
5  */
6 #include "usbh_core.h"
7 #include "usbh_msc.h"
8 #include "usb_scsi.h"
9 
10 #undef USB_DBG_TAG
11 #define USB_DBG_TAG "usbh_msc"
12 #include "usb_log.h"
13 
14 #define DEV_FORMAT "/dev/sd%c"
15 
16 #ifndef CONFIG_USBHOST_MSC_READY_CHECK_TIMES
17 #define CONFIG_USBHOST_MSC_READY_CHECK_TIMES 10
18 #endif
19 
20 USB_NOCACHE_RAM_SECTION USB_MEM_ALIGNX uint8_t g_msc_cbw_csw[CONFIG_USBHOST_MAX_MSC_CLASS][USB_ALIGN_UP(64, CONFIG_USB_ALIGN_SIZE)];
21 USB_NOCACHE_RAM_SECTION USB_MEM_ALIGNX uint8_t g_msc_buf[CONFIG_USBHOST_MAX_MSC_CLASS][USB_ALIGN_UP(64, CONFIG_USB_ALIGN_SIZE)];
22 
23 static struct usbh_msc g_msc_class[CONFIG_USBHOST_MAX_MSC_CLASS];
24 static uint32_t g_devinuse = 0;
25 static struct usbh_msc_modeswitch_config *g_msc_modeswitch_config = NULL;
26 
usbh_msc_class_alloc(void)27 static struct usbh_msc *usbh_msc_class_alloc(void)
28 {
29     uint8_t devno;
30 
31     for (devno = 0; devno < CONFIG_USBHOST_MAX_MSC_CLASS; devno++) {
32         if ((g_devinuse & (1U << devno)) == 0) {
33             g_devinuse |= (1U << devno);
34             memset(&g_msc_class[devno], 0, sizeof(struct usbh_msc));
35             g_msc_class[devno].sdchar = 'a' + devno;
36             return &g_msc_class[devno];
37         }
38     }
39     return NULL;
40 }
41 
usbh_msc_class_free(struct usbh_msc * msc_class)42 static void usbh_msc_class_free(struct usbh_msc *msc_class)
43 {
44     uint8_t devno = msc_class->sdchar - 'a';
45 
46     if (devno < 32) {
47         g_devinuse &= ~(1U << devno);
48     }
49     memset(msc_class, 0, sizeof(struct usbh_msc));
50 }
51 
usbh_msc_get_maxlun(struct usbh_msc * msc_class,uint8_t * buffer)52 static int usbh_msc_get_maxlun(struct usbh_msc *msc_class, uint8_t *buffer)
53 {
54     struct usb_setup_packet *setup;
55 
56     if (!msc_class || !msc_class->hport) {
57         return -USB_ERR_INVAL;
58     }
59     setup = msc_class->hport->setup;
60 
61     setup->bmRequestType = USB_REQUEST_DIR_IN | USB_REQUEST_CLASS | USB_REQUEST_RECIPIENT_INTERFACE;
62     setup->bRequest = MSC_REQUEST_GET_MAX_LUN;
63     setup->wValue = 0;
64     setup->wIndex = msc_class->intf;
65     setup->wLength = 1;
66 
67     return usbh_control_transfer(msc_class->hport, setup, buffer);
68 }
69 
usbh_msc_cbw_dump(struct CBW * cbw)70 static void usbh_msc_cbw_dump(struct CBW *cbw)
71 {
72     int i;
73 
74     USB_LOG_DBG("CBW:\r\n");
75     USB_LOG_DBG("  signature: 0x%08x\r\n", (unsigned int)cbw->dSignature);
76     USB_LOG_DBG("  tag:       0x%08x\r\n", (unsigned int)cbw->dTag);
77     USB_LOG_DBG("  datlen:    0x%08x\r\n", (unsigned int)cbw->dDataLength);
78     USB_LOG_DBG("  flags:     0x%02x\r\n", cbw->bmFlags);
79     USB_LOG_DBG("  lun:       0x%02x\r\n", cbw->bLUN);
80     USB_LOG_DBG("  cblen:    0x%02x\r\n", cbw->bCBLength);
81 
82     USB_LOG_DBG("CB:\r\n");
83     for (i = 0; i < cbw->bCBLength; i += 8) {
84         USB_LOG_DBG("  0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x\r\n",
85                     cbw->CB[i], cbw->CB[i + 1], cbw->CB[i + 2],
86                     cbw->CB[i + 3], cbw->CB[i + 4], cbw->CB[i + 5],
87                     cbw->CB[i + 6], cbw->CB[i + 7]);
88     }
89 }
90 
usbh_msc_csw_dump(struct CSW * csw)91 static void usbh_msc_csw_dump(struct CSW *csw)
92 {
93     (void)csw;
94 
95     USB_LOG_DBG("CSW:\r\n");
96     USB_LOG_DBG("  signature: 0x%08x\r\n", (unsigned int)csw->dSignature);
97     USB_LOG_DBG("  tag:       0x%08x\r\n", (unsigned int)csw->dTag);
98     USB_LOG_DBG("  residue:   0x%08x\r\n", (unsigned int)csw->dDataResidue);
99     USB_LOG_DBG("  status:    0x%02x\r\n", csw->bStatus);
100 }
101 
usbh_msc_bulk_in_transfer(struct usbh_msc * msc_class,uint8_t * buffer,uint32_t buflen,uint32_t timeout)102 static inline int usbh_msc_bulk_in_transfer(struct usbh_msc *msc_class, uint8_t *buffer, uint32_t buflen, uint32_t timeout)
103 {
104     int ret;
105     struct usbh_urb *urb = &msc_class->bulkin_urb;
106 
107     usbh_bulk_urb_fill(urb, msc_class->hport, msc_class->bulkin, buffer, buflen, timeout, NULL, NULL);
108     ret = usbh_submit_urb(urb);
109     if (ret == 0) {
110         ret = urb->actual_length;
111     }
112     return ret;
113 }
114 
usbh_msc_bulk_out_transfer(struct usbh_msc * msc_class,uint8_t * buffer,uint32_t buflen,uint32_t timeout)115 static inline int usbh_msc_bulk_out_transfer(struct usbh_msc *msc_class, uint8_t *buffer, uint32_t buflen, uint32_t timeout)
116 {
117     int ret;
118     struct usbh_urb *urb = &msc_class->bulkout_urb;
119 
120     usbh_bulk_urb_fill(urb, msc_class->hport, msc_class->bulkout, buffer, buflen, timeout, NULL, NULL);
121     ret = usbh_submit_urb(urb);
122     if (ret == 0) {
123         ret = urb->actual_length;
124     }
125     return ret;
126 }
127 
usbh_bulk_cbw_csw_xfer(struct usbh_msc * msc_class,struct CBW * cbw,struct CSW * csw,uint8_t * buffer,uint32_t timeout)128 static int usbh_bulk_cbw_csw_xfer(struct usbh_msc *msc_class, struct CBW *cbw, struct CSW *csw, uint8_t *buffer, uint32_t timeout)
129 {
130     int nbytes;
131 
132     usbh_msc_cbw_dump(cbw);
133 
134     /* Send the CBW */
135     nbytes = usbh_msc_bulk_out_transfer(msc_class, (uint8_t *)cbw, USB_SIZEOF_MSC_CBW, timeout);
136     if (nbytes < 0) {
137         USB_LOG_ERR("cbw transfer error: %d\r\n", nbytes);
138         goto __err_exit;
139     }
140 
141     if (cbw->dDataLength != 0) {
142         if (cbw->CB[0] == SCSI_CMD_WRITE10) {
143             nbytes = usbh_msc_bulk_out_transfer(msc_class, buffer, cbw->dDataLength, timeout);
144         } else if (cbw->CB[0] == SCSI_CMD_READCAPACITY10) {
145             nbytes = usbh_msc_bulk_in_transfer(msc_class, buffer, cbw->dDataLength, timeout);
146             if (nbytes >= 0) {
147                 /* Save the capacity information */
148                 msc_class->blocknum = GET_BE32(&buffer[0]) + 1;
149                 msc_class->blocksize = GET_BE32(&buffer[4]);
150             }
151         } else {
152             nbytes = usbh_msc_bulk_in_transfer(msc_class, buffer, cbw->dDataLength, timeout);
153         }
154 
155         if (nbytes < 0) {
156             USB_LOG_ERR("msc data transfer error: %d\r\n", nbytes);
157             goto __err_exit;
158         }
159     }
160 
161     /* Receive the CSW */
162     memset(csw, 0, USB_SIZEOF_MSC_CSW);
163     nbytes = usbh_msc_bulk_in_transfer(msc_class, (uint8_t *)csw, USB_SIZEOF_MSC_CSW, timeout);
164     if (nbytes < 0) {
165         USB_LOG_ERR("csw transfer error: %d\r\n", nbytes);
166         goto __err_exit;
167     }
168 
169     usbh_msc_csw_dump(csw);
170 
171     /* check csw status */
172     if (csw->dSignature != MSC_CSW_Signature) {
173         USB_LOG_ERR("csw signature error\r\n");
174         return -USB_ERR_INVAL;
175     }
176 
177     if (csw->bStatus != 0) {
178         USB_LOG_ERR("csw bStatus %d\r\n", csw->bStatus);
179         return -USB_ERR_INVAL;
180     }
181 __err_exit:
182     return nbytes < 0 ? (int)nbytes : 0;
183 }
184 
usbh_msc_scsi_testunitready(struct usbh_msc * msc_class)185 static inline int usbh_msc_scsi_testunitready(struct usbh_msc *msc_class)
186 {
187     struct CBW *cbw;
188 
189     /* Construct the CBW */
190     cbw = (struct CBW *)g_msc_cbw_csw[msc_class->sdchar - 'a'];
191     memset(cbw, 0, USB_SIZEOF_MSC_CBW);
192     cbw->dSignature = MSC_CBW_Signature;
193 
194     cbw->bCBLength = SCSICMD_TESTUNITREADY_SIZEOF;
195     cbw->CB[0] = SCSI_CMD_TESTUNITREADY;
196 
197     return usbh_bulk_cbw_csw_xfer(msc_class, cbw, (struct CSW *)g_msc_cbw_csw[msc_class->sdchar - 'a'], NULL, CONFIG_USBHOST_MSC_TIMEOUT);
198 }
199 
usbh_msc_scsi_requestsense(struct usbh_msc * msc_class)200 static inline int usbh_msc_scsi_requestsense(struct usbh_msc *msc_class)
201 {
202     struct CBW *cbw;
203 
204     /* Construct the CBW */
205     cbw = (struct CBW *)g_msc_cbw_csw[msc_class->sdchar - 'a'];
206     memset(cbw, 0, USB_SIZEOF_MSC_CBW);
207     cbw->dSignature = MSC_CBW_Signature;
208 
209     cbw->bmFlags = 0x80;
210     cbw->dDataLength = SCSIRESP_FIXEDSENSEDATA_SIZEOF;
211     cbw->bCBLength = SCSICMD_REQUESTSENSE_SIZEOF;
212     cbw->CB[0] = SCSI_CMD_REQUESTSENSE;
213     cbw->CB[4] = SCSIRESP_FIXEDSENSEDATA_SIZEOF;
214 
215     return usbh_bulk_cbw_csw_xfer(msc_class, cbw, (struct CSW *)g_msc_cbw_csw[msc_class->sdchar - 'a'], g_msc_buf[msc_class->sdchar - 'a'], CONFIG_USBHOST_MSC_TIMEOUT);
216 }
217 
usbh_msc_scsi_inquiry(struct usbh_msc * msc_class)218 static inline int usbh_msc_scsi_inquiry(struct usbh_msc *msc_class)
219 {
220     struct CBW *cbw;
221 
222     /* Construct the CBW */
223     cbw = (struct CBW *)g_msc_cbw_csw[msc_class->sdchar - 'a'];
224     memset(cbw, 0, USB_SIZEOF_MSC_CBW);
225     cbw->dSignature = MSC_CBW_Signature;
226 
227     cbw->dDataLength = SCSIRESP_INQUIRY_SIZEOF;
228     cbw->bmFlags = 0x80;
229     cbw->bCBLength = SCSICMD_INQUIRY_SIZEOF;
230     cbw->CB[0] = SCSI_CMD_INQUIRY;
231     cbw->CB[4] = SCSIRESP_INQUIRY_SIZEOF;
232 
233     return usbh_bulk_cbw_csw_xfer(msc_class, cbw, (struct CSW *)g_msc_cbw_csw[msc_class->sdchar - 'a'], g_msc_buf[msc_class->sdchar - 'a'], CONFIG_USBHOST_MSC_TIMEOUT);
234 }
235 
usbh_msc_scsi_readcapacity10(struct usbh_msc * msc_class)236 static inline int usbh_msc_scsi_readcapacity10(struct usbh_msc *msc_class)
237 {
238     struct CBW *cbw;
239 
240     /* Construct the CBW */
241     cbw = (struct CBW *)g_msc_cbw_csw[msc_class->sdchar - 'a'];
242     memset(cbw, 0, USB_SIZEOF_MSC_CBW);
243     cbw->dSignature = MSC_CBW_Signature;
244 
245     cbw->dDataLength = SCSIRESP_READCAPACITY10_SIZEOF;
246     cbw->bmFlags = 0x80;
247     cbw->bCBLength = SCSICMD_READCAPACITY10_SIZEOF;
248     cbw->CB[0] = SCSI_CMD_READCAPACITY10;
249 
250     return usbh_bulk_cbw_csw_xfer(msc_class, cbw, (struct CSW *)g_msc_cbw_csw[msc_class->sdchar - 'a'], g_msc_buf[msc_class->sdchar - 'a'], CONFIG_USBHOST_MSC_TIMEOUT);
251 }
252 
usbh_msc_modeswitch(struct usbh_msc * msc_class,const uint8_t * message)253 static inline void usbh_msc_modeswitch(struct usbh_msc *msc_class, const uint8_t *message)
254 {
255     struct CBW *cbw;
256 
257     /* Construct the CBW */
258     cbw = (struct CBW *)g_msc_cbw_csw[msc_class->sdchar - 'a'];
259 
260     memcpy(g_msc_cbw_csw[msc_class->sdchar - 'a'], message, 31);
261 
262     usbh_bulk_cbw_csw_xfer(msc_class, cbw, (struct CSW *)g_msc_cbw_csw[msc_class->sdchar - 'a'], NULL, CONFIG_USBHOST_MSC_TIMEOUT);
263 }
264 
usbh_msc_connect(struct usbh_hubport * hport,uint8_t intf)265 static int usbh_msc_connect(struct usbh_hubport *hport, uint8_t intf)
266 {
267     struct usb_endpoint_descriptor *ep_desc;
268     struct usbh_msc_modeswitch_config *config;
269     int ret;
270 
271     struct usbh_msc *msc_class = usbh_msc_class_alloc();
272     if (msc_class == NULL) {
273         USB_LOG_ERR("Fail to alloc msc_class\r\n");
274         return -USB_ERR_NOMEM;
275     }
276 
277     msc_class->hport = hport;
278     msc_class->intf = intf;
279 
280     hport->config.intf[intf].priv = msc_class;
281 
282     ret = usbh_msc_get_maxlun(msc_class, g_msc_buf[msc_class->sdchar - 'a']);
283     if (ret < 0) {
284         if (ret == -USB_ERR_STALL) {
285             USB_LOG_WRN("Device does not support multiple LUNs\r\n");
286             g_msc_buf[msc_class->sdchar - 'a'][0] = 0;
287             ret = 0;
288         } else {
289             return ret;
290         }
291     }
292 
293     USB_LOG_INFO("Get max LUN:%u\r\n", g_msc_buf[msc_class->sdchar - 'a'][0] + 1);
294 
295     for (uint8_t i = 0; i < hport->config.intf[intf].altsetting[0].intf_desc.bNumEndpoints; i++) {
296         ep_desc = &hport->config.intf[intf].altsetting[0].ep[i].ep_desc;
297         if (ep_desc->bEndpointAddress & 0x80) {
298             USBH_EP_INIT(msc_class->bulkin, ep_desc);
299         } else {
300             USBH_EP_INIT(msc_class->bulkout, ep_desc);
301         }
302     }
303 
304     if (g_msc_modeswitch_config) {
305         uint8_t num = 0;
306         while (1) {
307             config = &g_msc_modeswitch_config[num];
308             if (config && config->name) {
309                 if ((hport->device_desc.idVendor == config->vid) &&
310                     (hport->device_desc.idProduct == config->pid)) {
311                     USB_LOG_INFO("%s usb_modeswitch enable\r\n", config->name);
312                     usbh_msc_modeswitch(msc_class, config->message_content);
313                     return 0;
314                 }
315                 num++;
316             } else {
317                 break;
318             }
319         }
320     }
321 
322     snprintf(hport->config.intf[intf].devname, CONFIG_USBHOST_DEV_NAMELEN, DEV_FORMAT, msc_class->sdchar);
323 
324     USB_LOG_INFO("Register MSC Class:%s\r\n", hport->config.intf[intf].devname);
325 
326     usbh_msc_run(msc_class);
327     return ret;
328 }
329 
usbh_msc_disconnect(struct usbh_hubport * hport,uint8_t intf)330 static int usbh_msc_disconnect(struct usbh_hubport *hport, uint8_t intf)
331 {
332     int ret = 0;
333 
334     struct usbh_msc *msc_class = (struct usbh_msc *)hport->config.intf[intf].priv;
335 
336     if (msc_class) {
337         if (msc_class->bulkin) {
338             usbh_kill_urb(&msc_class->bulkin_urb);
339         }
340 
341         if (msc_class->bulkout) {
342             usbh_kill_urb(&msc_class->bulkout_urb);
343         }
344 
345         if (hport->config.intf[intf].devname[0] != '\0') {
346             usb_osal_thread_schedule_other();
347             USB_LOG_INFO("Unregister MSC Class:%s\r\n", hport->config.intf[intf].devname);
348             usbh_msc_stop(msc_class);
349         }
350 
351         usbh_msc_class_free(msc_class);
352     }
353 
354     return ret;
355 }
356 
usbh_msc_scsi_init(struct usbh_msc * msc_class)357 int usbh_msc_scsi_init(struct usbh_msc *msc_class)
358 {
359     int ret;
360     uint16_t cnt;
361 
362     cnt = 0;
363     while (usbh_msc_scsi_testunitready(msc_class) < 0) {
364         USB_LOG_WRN("Device not ready, try again...\r\n");
365         ret = usbh_msc_scsi_requestsense(msc_class);
366         if (ret < 0) {
367             USB_LOG_ERR("Fail to scsi_testunitready\r\n");
368         }
369         cnt++;
370         if (cnt > CONFIG_USBHOST_MSC_READY_CHECK_TIMES) {
371             return -USB_ERR_NODEV;
372         }
373     }
374     ret = usbh_msc_scsi_inquiry(msc_class);
375     if (ret < 0) {
376         USB_LOG_ERR("Fail to scsi_inquiry\r\n");
377         return ret;
378     }
379 
380     ret = usbh_msc_scsi_readcapacity10(msc_class);
381     if (ret < 0) {
382         USB_LOG_ERR("Fail to scsi_readcapacity10\r\n");
383         return ret;
384     }
385 
386     if (msc_class->blocksize > 0) {
387         USB_LOG_INFO("Capacity info:\r\n");
388         USB_LOG_INFO("Block num:%d,block size:%d\r\n", (unsigned int)msc_class->blocknum, (unsigned int)msc_class->blocksize);
389     } else {
390         USB_LOG_ERR("Invalid block size\r\n");
391         return -USB_ERR_RANGE;
392     }
393 
394     return 0;
395 }
396 
usbh_msc_scsi_write10(struct usbh_msc * msc_class,uint32_t start_sector,const uint8_t * buffer,uint32_t nsectors)397 int usbh_msc_scsi_write10(struct usbh_msc *msc_class, uint32_t start_sector, const uint8_t *buffer, uint32_t nsectors)
398 {
399     struct CBW *cbw;
400 
401     /* Construct the CBW */
402     cbw = (struct CBW *)g_msc_cbw_csw[msc_class->sdchar - 'a'];
403     memset(cbw, 0, USB_SIZEOF_MSC_CBW);
404     cbw->dSignature = MSC_CBW_Signature;
405 
406     cbw->dDataLength = (msc_class->blocksize * nsectors);
407     cbw->bCBLength = SCSICMD_WRITE10_SIZEOF;
408     cbw->CB[0] = SCSI_CMD_WRITE10;
409 
410     SET_BE32(&cbw->CB[2], start_sector);
411     SET_BE16(&cbw->CB[7], nsectors);
412 
413     return usbh_bulk_cbw_csw_xfer(msc_class, cbw, (struct CSW *)g_msc_cbw_csw[msc_class->sdchar - 'a'], (uint8_t *)buffer, CONFIG_USBHOST_MSC_TIMEOUT);
414 }
415 
usbh_msc_scsi_read10(struct usbh_msc * msc_class,uint32_t start_sector,const uint8_t * buffer,uint32_t nsectors)416 int usbh_msc_scsi_read10(struct usbh_msc *msc_class, uint32_t start_sector, const uint8_t *buffer, uint32_t nsectors)
417 {
418     struct CBW *cbw;
419 
420     /* Construct the CBW */
421     cbw = (struct CBW *)g_msc_cbw_csw[msc_class->sdchar - 'a'];
422     memset(cbw, 0, USB_SIZEOF_MSC_CBW);
423     cbw->dSignature = MSC_CBW_Signature;
424 
425     cbw->dDataLength = (msc_class->blocksize * nsectors);
426     cbw->bmFlags = 0x80;
427     cbw->bCBLength = SCSICMD_READ10_SIZEOF;
428     cbw->CB[0] = SCSI_CMD_READ10;
429 
430     SET_BE32(&cbw->CB[2], start_sector);
431     SET_BE16(&cbw->CB[7], nsectors);
432 
433     return usbh_bulk_cbw_csw_xfer(msc_class, cbw, (struct CSW *)g_msc_cbw_csw[msc_class->sdchar - 'a'], (uint8_t *)buffer, CONFIG_USBHOST_MSC_TIMEOUT);
434 }
435 
usbh_msc_modeswitch_enable(struct usbh_msc_modeswitch_config * config)436 void usbh_msc_modeswitch_enable(struct usbh_msc_modeswitch_config *config)
437 {
438     if (config) {
439         g_msc_modeswitch_config = config;
440     } else {
441         g_msc_modeswitch_config = NULL;
442     }
443 }
444 
usbh_msc_run(struct usbh_msc * msc_class)445 __WEAK void usbh_msc_run(struct usbh_msc *msc_class)
446 {
447     (void)msc_class;
448 }
449 
usbh_msc_stop(struct usbh_msc * msc_class)450 __WEAK void usbh_msc_stop(struct usbh_msc *msc_class)
451 {
452     (void)msc_class;
453 }
454 
455 const struct usbh_class_driver msc_class_driver = {
456     .driver_name = "msc",
457     .connect = usbh_msc_connect,
458     .disconnect = usbh_msc_disconnect
459 };
460 
461 CLASS_INFO_DEFINE const struct usbh_class_info msc_class_info = {
462     .match_flags = USB_CLASS_MATCH_INTF_CLASS | USB_CLASS_MATCH_INTF_SUBCLASS | USB_CLASS_MATCH_INTF_PROTOCOL,
463     .bInterfaceClass = USB_DEVICE_CLASS_MASS_STORAGE,
464     .bInterfaceSubClass = MSC_SUBCLASS_SCSI,
465     .bInterfaceProtocol = MSC_PROTOCOL_BULK_ONLY,
466     .id_table = NULL,
467     .class_driver = &msc_class_driver
468 };
469