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