/* * usb-msc.c -- USB Mass Storage Class protocol handling * * Copyright (C) 2011, 2012 Free Software Initiative of Japan * Author: NIIBE Yutaka * * This file is a part of Gnuk, a GnuPG USB Token implementation. * * Gnuk is free software: you can redistribute it and/or modify it * under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * Gnuk is distributed in the hope that it will be useful, but WITHOUT * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public * License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . * */ #include "config.h" #include "ch.h" #include "gnuk.h" #include "usb_lld.h" #include "usb-msc.h" struct usb_endp_in { const uint8_t *txbuf; /* Pointer to the transmission buffer. */ size_t txsize; /* Transmit transfer size remained. */ size_t txcnt; /* Transmitted bytes so far. */ }; struct usb_endp_out { uint8_t *rxbuf; /* Pointer to the receive buffer. */ size_t rxsize; /* Requested receive transfer size. */ size_t rxcnt; /* Received bytes so far. */ }; static struct usb_endp_in ep6_in; static struct usb_endp_out ep6_out; static Thread *the_thread; #define ENDP_MAX_SIZE 64 static uint8_t msc_state; static void usb_start_transmit (const uint8_t *p, size_t n) { size_t pkt_len = n > ENDP_MAX_SIZE ? ENDP_MAX_SIZE : n; ep6_in.txbuf = p; ep6_in.txsize = n; ep6_in.txcnt = 0; usb_lld_write (ENDP6, (uint8_t *)ep6_in.txbuf, pkt_len); } /* "Data Transmitted" callback */ void EP6_IN_Callback (void) { size_t n = (size_t)usb_lld_tx_data_len (ENDP6); ep6_in.txbuf += n; ep6_in.txcnt += n; ep6_in.txsize -= n; if (ep6_in.txsize > 0) /* More data to be sent */ { if (ep6_in.txsize > ENDP_MAX_SIZE) n = ENDP_MAX_SIZE; else n = ep6_in.txsize; usb_lld_write (ENDP6, (uint8_t *)ep6_in.txbuf, n); return; } /* Transmit has been completed, notify the waiting thread */ switch (msc_state) { case MSC_SENDING_CSW: case MSC_DATA_IN: if (the_thread != NULL) { Thread *tp; chSysLockFromIsr (); tp = the_thread; the_thread = NULL; tp->p_u.rdymsg = RDY_OK; chSchReadyI (tp); chSysUnlockFromIsr (); } break; default: break; } } static void usb_start_receive (uint8_t *p, size_t n) { ep6_out.rxbuf = p; ep6_out.rxsize = n; ep6_out.rxcnt = 0; usb_lld_rx_enable (ENDP6); } /* "Data Received" call back */ void EP6_OUT_Callback (void) { size_t n = (size_t)usb_lld_rx_data_len (ENDP6); int err = 0; if (n > ep6_out.rxsize) { /* buffer overflow */ err = 1; n = ep6_out.rxsize; } usb_lld_rxcpy (ep6_out.rxbuf, ENDP6, 0, n); ep6_out.rxbuf += n; ep6_out.rxcnt += n; ep6_out.rxsize -= n; if (n == ENDP_MAX_SIZE && ep6_out.rxsize != 0) { /* More data to be received */ usb_lld_rx_enable (ENDP6); return; } /* Receiving has been completed, notify the waiting thread */ switch (msc_state) { case MSC_IDLE: case MSC_DATA_OUT: if (the_thread != NULL) { Thread *tp; chSysLockFromIsr (); tp = the_thread; the_thread = NULL; tp->p_u.rdymsg = err? RDY_RESET : RDY_OK; chSchReadyI (tp); chSysUnlockFromIsr (); } break; default: break; } } static const uint8_t scsi_inquiry_data_00[] = { 0, 0, 0, 0, 0 }; static const uint8_t scsi_inquiry_data[] = { 0x00, /* Direct Access Device. */ 0x80, /* RMB = 1: Removable Medium. */ 0x05, /* Version: SPC-3. */ 0x02, /* Response format: SPC-3. */ 36 - 4, /* Additional Length. */ 0x00, 0x00, 0x00, /* Vendor Identification */ 'F', 'S', 'I', 'J', ' ', ' ', ' ', ' ', /* Product Identification */ 'V', 'i', 'r', 't', 'u', 'a', 'l', ' ', 'D', 'i', 's', 'k', ' ', ' ', ' ', ' ', /* Product Revision Level */ '1', '.', '0', ' ' }; static uint8_t scsi_sense_data_desc[] = { 0x72, /* Response Code: descriptor, current */ 0x02, /* Sense Key */ 0x3a, /* ASC (additional sense code) */ 0x00, /* ASCQ (additional sense code qualifier) */ 0x00, 0x00, 0x00, 0x00, /* Additional Sense Length */ }; static uint8_t scsi_sense_data_fixed[] = { 0x70, /* Response Code: fixed, current */ 0x00, 0x02, /* Sense Key */ 0x00, 0x00, 0x00, 0x00, 0x0a, /* Additional Sense Length */ 0x00, 0x00, 0x00, 0x00, 0x3a, /* ASC (additional sense code) */ 0x00, /* ASCQ (additional sense code qualifier) */ 0x00, 0x00, 0x00, 0x00, }; static void set_scsi_sense_data(uint8_t sense_key, uint8_t asc) { scsi_sense_data_desc[1] = scsi_sense_data_fixed[2] = sense_key; scsi_sense_data_desc[2] = scsi_sense_data_fixed[12] = asc; } static uint8_t buf[512]; static uint8_t contingent_allegiance; static uint8_t keep_contingent_allegiance; uint8_t media_available; void msc_media_insert_change (int available) { contingent_allegiance = 1; media_available = available; if (available) { set_scsi_sense_data (0x06, 0x28); /* UNIT_ATTENTION */ keep_contingent_allegiance = 0; } else { set_scsi_sense_data (0x02, 0x3a); /* NOT_READY */ keep_contingent_allegiance = 1; } } static uint8_t scsi_read_format_capacities (uint32_t *nblocks, uint32_t *secsize) { *nblocks = 68; *secsize = 512; if (media_available) return 2; /* Formatted Media.*/ else return 3; /* No Media.*/ } static struct CBW CBW; static struct CSW CSW; static int msc_recv_data (void) { msg_t msg; chSysLock (); msc_state = MSC_DATA_OUT; the_thread = chThdSelf (); usb_start_receive (buf, 512); chSchGoSleepS (THD_STATE_SUSPENDED); msg = chThdSelf ()->p_u.rdymsg; chSysUnlock (); return 0; } static void msc_send_data (const uint8_t *p, size_t n) { msg_t msg; chSysLock (); msc_state = MSC_DATA_IN; the_thread = chThdSelf (); usb_start_transmit (p, n); chSchGoSleepS (THD_STATE_SUSPENDED); msg = chThdSelf ()->p_u.rdymsg; CSW.dCSWDataResidue -= (uint32_t)n; chSysUnlock(); } static void msc_send_result (const uint8_t *p, size_t n) { msg_t msg; if (p != NULL) { if (n > CBW.dCBWDataTransferLength) n = CBW.dCBWDataTransferLength; CSW.dCSWDataResidue = CBW.dCBWDataTransferLength; msc_send_data (p, n); CSW.bCSWStatus = MSC_CSW_STATUS_PASSED; } CSW.dCSWSignature = MSC_CSW_SIGNATURE; chSysLock (); msc_state = MSC_SENDING_CSW; the_thread = chThdSelf (); usb_start_transmit ((uint8_t *)&CSW, sizeof CSW); chSchGoSleepS (THD_STATE_SUSPENDED); msg = chThdSelf ()->p_u.rdymsg; chSysUnlock (); } void msc_handle_command (void) { size_t n; uint32_t nblocks, secsize; uint32_t lba; int r; msg_t msg; chSysLock(); msc_state = MSC_IDLE; the_thread = chThdSelf (); usb_start_receive ((uint8_t *)&CBW, sizeof CBW); chSchGoSleepTimeoutS (THD_STATE_SUSPENDED, MS2ST (1000)); msg = chThdSelf ()->p_u.rdymsg; chSysUnlock (); if (msg != RDY_OK) { /* Error occured, ignore the request and go into error state */ msc_state = MSC_ERROR; if (msg != RDY_TIMEOUT) { chSysLock (); usb_lld_stall_rx (ENDP6); chSysUnlock (); } return; } n = ep6_out.rxcnt; if ((n != sizeof (struct CBW)) || (CBW.dCBWSignature != MSC_CBW_SIGNATURE)) { msc_state = MSC_ERROR; chSysLock (); usb_lld_stall_rx (ENDP6); chSysUnlock (); return; } CSW.dCSWTag = CBW.dCBWTag; switch (CBW.CBWCB[0]) { case SCSI_REQUEST_SENSE: if (CBW.CBWCB[1] & 0x01) /* DESC */ msc_send_result ((uint8_t *)&scsi_sense_data_desc, sizeof scsi_sense_data_desc); else msc_send_result ((uint8_t *)&scsi_sense_data_fixed, sizeof scsi_sense_data_fixed); /* After the error is reported, clear it, if it's . */ if (!keep_contingent_allegiance) { contingent_allegiance = 0; set_scsi_sense_data (0x00, 0x00); } return; case SCSI_INQUIRY: if (CBW.CBWCB[1] & 0x01) /* EVPD */ /* assume page 00 */ msc_send_result ((uint8_t *)&scsi_inquiry_data_00, sizeof scsi_inquiry_data_00); else msc_send_result ((uint8_t *)&scsi_inquiry_data, sizeof scsi_inquiry_data); return; case SCSI_READ_FORMAT_CAPACITIES: buf[8] = scsi_read_format_capacities (&nblocks, &secsize); buf[0] = buf[1] = buf[2] = 0; buf[3] = 8; buf[4] = (uint8_t)(nblocks >> 24); buf[5] = (uint8_t)(nblocks >> 16); buf[6] = (uint8_t)(nblocks >> 8); buf[7] = (uint8_t)(nblocks >> 0); buf[9] = (uint8_t)(secsize >> 16); buf[10] = (uint8_t)(secsize >> 8); buf[11] = (uint8_t)(secsize >> 0); msc_send_result (buf, 12); return; case SCSI_START_STOP_UNIT: if (CBW.CBWCB[4] == 0x00 /* stop */ || CBW.CBWCB[4] == 0x02 /* eject */ || CBW.CBWCB[4] == 0x03 /* close */) { msc_scsi_stop (CBW.CBWCB[4]); set_scsi_sense_data (0x05, 0x24); /* ILLEGAL_REQUEST */ contingent_allegiance = 1; keep_contingent_allegiance = 1; } /* CBW.CBWCB[4] == 0x01 *//* start */ goto success; case SCSI_TEST_UNIT_READY: if (contingent_allegiance) { CSW.bCSWStatus = MSC_CSW_STATUS_FAILED; CSW.dCSWDataResidue = 0; msc_send_result (NULL, 0); return; } /* fall through */ success: case SCSI_SYNCHRONIZE_CACHE: case SCSI_VERIFY10: case SCSI_ALLOW_MEDIUM_REMOVAL: CSW.bCSWStatus = MSC_CSW_STATUS_PASSED; CSW.dCSWDataResidue = CBW.dCBWDataTransferLength; msc_send_result (NULL, 0); return; case SCSI_MODE_SENSE6: buf[0] = 0x03; buf[1] = buf[2] = buf[3] = 0; msc_send_result (buf, 4); return; case SCSI_READ_CAPACITY10: scsi_read_format_capacities (&nblocks, &secsize); buf[0] = (uint8_t)((nblocks - 1) >> 24); buf[1] = (uint8_t)((nblocks - 1) >> 16); buf[2] = (uint8_t)((nblocks - 1) >> 8); buf[3] = (uint8_t)((nblocks - 1) >> 0); buf[4] = (uint8_t)(secsize >> 24); buf[5] = (uint8_t)(secsize >> 16); buf[6] = (uint8_t)(secsize >> 8); buf[7] = (uint8_t)(secsize >> 0); msc_send_result (buf, 8); return; case SCSI_READ10: case SCSI_WRITE10: break; default: if (CBW.dCBWDataTransferLength == 0) { CSW.bCSWStatus = MSC_CSW_STATUS_FAILED; CSW.dCSWDataResidue = 0; msc_send_result (NULL, 0); return; } else { msc_state = MSC_ERROR; chSysLock (); usb_lld_stall_tx (ENDP6); usb_lld_stall_rx (ENDP6); chSysUnlock (); return; } } lba = (CBW.CBWCB[2] << 24) | (CBW.CBWCB[3] << 16) | (CBW.CBWCB[4] << 8) | CBW.CBWCB[5]; /* Transfer direction.*/ if (CBW.bmCBWFlags & 0x80) { /* IN, Device to Host.*/ msc_state = MSC_DATA_IN; if (CBW.CBWCB[0] == SCSI_READ10) { const uint8_t *p; CSW.dCSWDataResidue = 0; while (1) { if (CBW.CBWCB[7] == 0 && CBW.CBWCB[8] == 0) { CSW.bCSWStatus = MSC_CSW_STATUS_PASSED; break; } if ((r = msc_scsi_read (lba, &p)) == 0) { msc_send_data (p, 512); if (++CBW.CBWCB[5] == 0) if (++CBW.CBWCB[4] == 0) if (++CBW.CBWCB[3] == 0) ++CBW.CBWCB[2]; if (CBW.CBWCB[8]-- == 0) CBW.CBWCB[7]--; CSW.dCSWDataResidue += 512; } else { CSW.bCSWStatus = MSC_CSW_STATUS_FAILED; contingent_allegiance = 1; if (r == SCSI_ERROR_NOT_READY) set_scsi_sense_data (SCSI_ERROR_NOT_READY, 0x3a); else set_scsi_sense_data (r, 0x00); break; } } msc_send_result (NULL, 0); } } else { /* OUT, Host to Device.*/ if (CBW.CBWCB[0] == SCSI_WRITE10) { CSW.dCSWDataResidue = CBW.dCBWDataTransferLength; while (1) { if (CBW.CBWCB[8] == 0 && CBW.CBWCB[7] == 0) { CSW.bCSWStatus = MSC_CSW_STATUS_PASSED; break; } msc_recv_data (); if ((r = msc_scsi_write (lba, buf, 512)) == 0) { if (++CBW.CBWCB[5] == 0) if (++CBW.CBWCB[4] == 0) if (++CBW.CBWCB[3] == 0) ++CBW.CBWCB[2]; if (CBW.CBWCB[8]-- == 0) CBW.CBWCB[7]--; CSW.dCSWDataResidue -= 512; } else { CSW.bCSWStatus = MSC_CSW_STATUS_FAILED; contingent_allegiance = 1; if (r == SCSI_ERROR_NOT_READY) set_scsi_sense_data (SCSI_ERROR_NOT_READY, 0x3a); else set_scsi_sense_data (r, 0x00); break; } } msc_send_result (NULL, 0); } } } static msg_t msc_main (void *arg) { (void)arg; /* Initially, it starts with no media */ msc_media_insert_change (0); while (1) msc_handle_command (); return 0; } static WORKING_AREA(wa_msc_thread, 128); void msc_init (void) { chThdCreateStatic (wa_msc_thread, sizeof (wa_msc_thread), NORMALPRIO, msc_main, NULL); }