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