usbh_msc.c 14 KB

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