Index: firmware/usbstack/drivers/device/usb_storage.c
===================================================================
--- firmware/usbstack/drivers/device/usb_storage.c	(revision 15673)
+++ firmware/usbstack/drivers/device/usb_storage.c	(working copy)
@@ -69,8 +69,8 @@
     .bDeviceClass =         0,
     .bDeviceSubClass =      0,
     .bDeviceProtocol =      0,
-    .idVendor =             0xffff,
-    .idProduct =            0x0001,
+    .idVendor =             0x0b70,
+    .idProduct =            0x00ba,
     .iManufacturer =        MANUFACTURER_STR_ID,
     .iProduct =             PRODUCT_STR_ID,
     .iSerialNumber =        SERIAL_STR_ID,
@@ -154,7 +154,7 @@
     NULL,
 };
 
-#define BUFFER_SIZE 100
+#define BUFFER_SIZE 512
 uint8_t buf[BUFFER_SIZE];
 
 struct usb_response res;
@@ -163,6 +163,7 @@
 static int config_buf(uint8_t *buf, uint8_t type, unsigned index);
 static int set_config(int config);
 static int set_interface_alt_setting(int interface_alt_setting);
+static int command_exec(struct usb_ep* ep, char* req);
 
 struct device {
     struct usb_ep* in;
@@ -175,6 +176,36 @@
 
 static struct device dev;
 
+#define MSD_IN_EP_SIZE      	512
+#define MSD_OUT_EP_SIZE     	512
+#define BLOCKLEN_512            0x0200
+
+byte MSD_State;			// Takes values MSD_WAIT, MSD_DATA_IN or MSD_DATA_OUT
+USB_MSD_CBW gblCBW;
+USB_MSD_CBW *msd_cbw;
+USB_MSD_CSW msd_csw;
+byte gblCBWLength;
+RequestSenseResponse gblSenseData;
+byte *ptrNextData;
+
+DWORD gblNumBLKS,gblBLKLen;	
+
+/* Standard Response to INQUIRY command stored 	*/
+const InquiryResponse inq_resp = {
+	0x00,		// peripheral device is connected, direct access block device
+	0x80,       // removable
+	0x04,	 	// version = 00=> does not conform to any standard, 4=> SPC-2
+	0x02,		// response is in format specified by SPC-2
+	0x20,		// n-4 = 36-4=32= 0x20
+	0x00,		// sccs etc.
+	0x00,		// bque=1 and cmdque=0,indicates simple queueing 00 is obsolete,
+				// but as in case of other device, we are just using 00
+	0x00,		// 00 obsolete, 0x80 for basic task queueing
+	"Rockbox ",	// this is the T10 assigned Vendor ID
+	"Mass Storage    ",
+	"0001"
+};
+
 /*-------------------------------------------------------------------------*/
 
 void usb_storage_driver_init(void)
@@ -285,6 +316,7 @@
         case USB_REQ_SET_CONFIGURATION:
             logf("usb storage: set configuration %d", request->wValue);
             ret = set_config(request->wValue);
+	    return ret;
             break;
 
         case USB_REQ_GET_CONFIGURATION:
@@ -316,6 +348,7 @@
             logf("usb storage: get max lun");
             /* we support no LUNs (Logical Unit Number) */
             buf[0] = 0;
+	    res.buf = buf;
             ret = 1;
             break;
         }
@@ -367,14 +400,18 @@
 {
     /* enable endpoints */
     logf("setup %s", dev.in->name);
+    dev.in->command = command_exec;
     ops->enable(dev.in, (struct usb_endpoint_descriptor*)dev.descriptors[1]);
     logf("setup %s", dev.out->name);
+    dev.out->command = command_exec;
     ops->enable(dev.out, (struct usb_endpoint_descriptor*)dev.descriptors[2]);
 
     dev.used_config = config;
 
     /* setup buffers */
 
+    MSD_State=MSD_WAIT;
+	
     return 0;
 }
 
@@ -385,3 +422,810 @@
     return 0;
 }
 
+/* Updated from Microchip Mass Storage Device Driver: http://ww1.microchip.com/downloads/en/AppNotes/MCHPMSD.zip */
+/******************************************************************************
+ * Function:        void SendCSW(void)
+ *
+ * PreCondition:    None 
+ *
+ * Input:           None
+ *
+ * Output:          None
+ *
+ * Side Effects:    None
+ *
+ * Overview:        This function sends the CSW and sets the State to MSD_WAIT
+ *                  It also changes MSD_BD_OUT to point to msd_csw (structure
+ *                  for reading CSW) Note that this was changed in
+ *                  MSD_DATA_OUT state to point to buf in order to
+ *                  read data from host
+ *
+ * Note:            None
+ *****************************************************************************/	
+void SendCSW(void)
+{
+	res.length = MSD_CSW_SIZE;
+	res.buf = &msd_csw;
+	ops->send(dev.in, &res);
+	res.length = BUFFER_SIZE;
+	res.buf = buf;
+	ops->receive(dev.out, NULL);
+   	MSD_State=MSD_WAIT;
+}
+
+
+/******************************************************************************
+ * Function:        void SendData(byte* dataAddr, byte dataSize)
+ *
+ * PreCondition:    None 
+ *
+ * Input:           None
+ *
+ * Output:          None
+ *
+ * Side Effects:    None
+ *
+ * Overview:        This function sends "dataSize" bytes of data
+ *                  (< MSD_EP_IN_SIZE) starting at address "dataAddr".
+ *
+ * Note:            None
+ *****************************************************************************/	
+	
+void SendData(byte* dataAddr, int dataSize)
+{
+	res.length = dataSize;
+	res.buf = dataAddr;
+	ops->send(dev.in, &res);
+}
+
+/******************************************************************************
+ * Function:        void MSDDataIn(void)
+ *
+ * PreCondition:    None 
+ *
+ * Input:           None
+ *
+ * Output:          None
+ *
+ * Side Effects:    None
+ *
+ * Overview:        This function sends 512B of data in buf to the
+ *                  host in 64B chunks using MSD_BD_IN.Various conditions
+ *                  when data to be sent is less than MSD_IN_EP_SIZE and
+ *                  error condition bCSWStatus = 0x01 are checked. As per
+ *                  specifications, in case of error 0 filled data of the size 
+ *                  expected by the host dCBWDataTransferLength is sent.
+ *
+ * Note:            None
+ *****************************************************************************/	
+void MSDDataIn(void)
+{
+	byte i;
+	dword size;
+	/* Case (status==0) and (data to be sent > MSD_IN_EP_SIZE)*/
+	if ((msd_csw.bCSWStatus==0x00)&&(msd_csw.dCSWDataResidue>=MSD_IN_EP_SIZE)) {
+		/* Write next chunk of data to EP Buffer and send */
+		SendData(ptrNextData,MSD_IN_EP_SIZE);
+		gblCBW.dCBWDataTransferLength-=	MSD_IN_EP_SIZE;
+		msd_csw.dCSWDataResidue-=MSD_IN_EP_SIZE;
+		ptrNextData+=MSD_IN_EP_SIZE;
+	} else {
+		if (msd_csw.bCSWStatus!=0x0) { // error path status!=0
+			size=MIN(MSD_IN_EP_SIZE,gblCBW.dCBWDataTransferLength);
+			for (i=0;i<size;i++) buf[i]=0;	// prepare 0 data
+			if (gblCBW.dCBWDataTransferLength > MSD_IN_EP_SIZE) {	
+				/* Case (status!=0) and (data to be sent > MSD_IN_EP_SIZE)*/
+				/* write next chunk of data to EP Buffer and send */
+				SendData((byte*)&buf[0],MSD_IN_EP_SIZE);
+				gblCBW.dCBWDataTransferLength -= MSD_IN_EP_SIZE;
+				msd_csw.dCSWDataResidue-=MSD_IN_EP_SIZE;
+			} else {				
+				/* Case (status!=0) and (data to be sent < MSD_IN_EP_SIZE) */
+				/* write next chunk of data to EP Buffer and send*/
+				SendData((byte*)&buf[0],gblCBW.dCBWDataTransferLength);
+				gblCBW.dCBWDataTransferLength = 0;			
+				/* we have sent 0s for what was expected by host*/
+				msd_csw.dCSWDataResidue -= gblCBW.dCBWDataTransferLength;
+			}
+		} else { 					
+			/* Case (status ==0) and (data to be sent < MSD_IN_EP_SIZE) */
+			/* write next chunk of data to EP Buffer and send */
+			SendData(ptrNextData,msd_csw.dCSWDataResidue);
+			/* we have sent all the data that was expected by host */
+			gblCBW.dCBWDataTransferLength -= msd_csw.dCSWDataResidue ;	
+			msd_csw.dCSWDataResidue = gblCBW.dCBWDataTransferLength;
+			/* In case the host expected more than what we had to send */
+			/* Setting DataTransferLength=0 so that CSW is sent after this*/
+			gblCBW.dCBWDataTransferLength = 0;							
+													
+		}
+	}
+}
+	
+/******************************************************************************
+ * Function:        void IsValidCBW()
+ *
+ * PreCondition:    None 
+ *
+ * Input:           None
+ *
+ * Output:          None
+ *
+ * Side Effects:    None
+ *
+ * Overview:        This checks if the received CBW is valid
+ *                  According to the Mass Storage Class Specifications, 
+ *		    a CSW is considered to be valid if 
+ *                  1. It was received in MS_WAIT State 
+ *                  2. CBW length is 1Fh bytes (MSD_CBW_SIZE)
+ *                  3. dCBWSignature is equal to 0x43425355h		
+ *
+ * Note:            None
+ *****************************************************************************/	
+	
+byte IsValidCBW(void) 
+{
+	if ((gblCBWLength!=MSD_CBW_SIZE)||(gblCBW.dCBWSignature!=0x43425355))return FALSE;
+    else return TRUE;
+}
+
+/******************************************************************************
+ * Function:        void IsMeaningfulCBW()
+ *
+ * PreCondition:    None 
+ *
+ * Input:           None
+ *
+ * Output:          None
+ *
+ * Side Effects:    None
+ *
+ * Overview:        This checks if the received CBW is meaningful
+ *                  According to the Mass Storage Class Specifications, 
+ *		    a CSW is considered to be meaningful if 
+ *                  1. No reserved bits are set
+ *                  2. bCBWLUN contains a valid LUN supported by device
+ *		    3. bCBWCBLength and CBWCB are in accordance with
+ *                     bInterfaceSubClass
+ * Note:            None
+ *****************************************************************************/	
+	
+byte IsMeaningfulCBW(void)
+{
+	/*  3msb bits of CBWCBLength are reserved and must be 0,
+     *  4msb bits of CBWLUN	are reserved and must be 0
+	 *  valid CBWCBLength is between 1 and 16B
+	 *  In bCBWFlags only msb indicates data direction rest must be 0
+	 */  
+	if((gblCBW.bCBWLUN<=0x0f)&&(gblCBW.bCBWCBLength<=0x10)&&(gblCBW.bCBWCBLength>=0x01)&&((gblCBW.bCBWFlags==0x00)|(gblCBW.bCBWFlags==0x80))) 
+		return TRUE;
+	else return FALSE;	
+	
+}
+
+/******************************************************************************
+ * Function:        void PrepareCSWData()
+ *
+ * PreCondition:    None 
+ *
+ * Input:           None
+ *
+ * Output:          None
+ *
+ * Side Effects:    None
+ *
+ * Overview:        This prepares the Status data of CSW by copying the
+ *                  dCSWTag from CBWTage and sets the signature 
+ *                  of valid CSW=53425355h
+ *
+ * Note:            None
+ *****************************************************************************/	
+void PrepareCSWData(void)
+{ 
+	/* Residue and Status fields are set after
+       decoding and executing the command  */
+	msd_csw.dCSWTag=gblCBW.dCBWTag;
+	msd_csw.dCSWSignature=0x53425355;
+}
+
+
+/******************************************************************************
+ * Function:        void MSDInquiryHandler(void)
+ *
+ * PreCondition:    None 
+ *
+ * Input:           None
+ *
+ * Output:          None
+ *
+ * Side Effects:    None
+ *
+ * Overview:        This function prepares the response of the Inquiry command
+ *                  A fixed Inquiry response is copied from ROM to the
+ *		    buf and CSWStatus, CSWDataResidue values are set
+ *
+ * Note:            None
+ *****************************************************************************/	
+	
+void MSDInquiryHandler(void)
+{
+	memcpy((byte *)&buf[0],(byte *)&inq_resp,sizeof(InquiryResponse));
+	msd_csw.dCSWDataResidue=sizeof(InquiryResponse);
+	msd_csw.bCSWStatus=0x00;			// success
+	return;
+}
+
+
+/******************************************************************************
+ * Function:        void ResetSenseData(void)
+ *
+ * PreCondition:    None 
+ *
+ * Input:           None
+ *
+ * Output:          None
+ *
+ * Side Effects:    None
+ *
+ * Overview:        This routine resets the Sense Data, initializing the
+ *                  structure RequestSenseResponse gblSenseData.
+ *                  
+ *
+ * Note:            None
+ *****************************************************************************/
+void ResetSenseData(void) 
+	{
+	gblSenseData.ResponseCode=S_CURRENT;
+	gblSenseData.VALID=0;			// no data in the information field
+	gblSenseData.Obsolete=0x0;
+	gblSenseData.SenseKey=S_NO_SENSE;
+	gblSenseData.Resv=0;
+	gblSenseData.ILI=0;
+	gblSenseData.EOM=0;
+	gblSenseData.FILEMARK=0;
+	gblSenseData.Information._dword=0x00;
+	gblSenseData.AddSenseLen=0x0a;	// n-7 (n=17 (0..17))
+	gblSenseData.CmdSpecificInfo._dword=0x0;
+	gblSenseData.ASC=0x0;
+	gblSenseData.ASCQ=0x0;
+	gblSenseData.FRUC=0x0;
+	gblSenseData.SenseKeySpecific[0]=0x0;
+	gblSenseData.SenseKeySpecific[1]=0x0;
+	gblSenseData.SenseKeySpecific[2]=0x0;
+}
+
+void MSDReadFormatCapacityHandler(void)
+{
+/* To be refined */
+	buf[0]=0x00;
+	buf[1]=0x00;
+	buf[2]=0x00;
+	buf[3]=0x08;
+	buf[4]=0x02;
+	buf[5]=0x54;
+	buf[6]=0x29;
+	buf[7]=0x80;
+	buf[8]=0x02;
+	buf[9]=0x00;
+	buf[10]=0x02;
+	buf[11]=0x00;
+
+	msd_csw.dCSWDataResidue=0x0c;		  // size of response
+	msd_csw.bCSWStatus=0x00;			  // success
+}
+
+/******************************************************************************
+ * Function:        void MSDReadCapacityHandler()
+ *
+ * PreCondition:    None 
+ *
+ * Input:           None
+ *
+ * Output:          None
+ *
+ * Side Effects:    None
+ *
+ * Overview:        This function processes the data from CSD register
+ *                  (read during intiailization of sdcard)to find the number
+ *                  of blocks (gblNumBLKS) and block length (gblBLKLen)
+ *                  This data is then copied to buf and a response
+ *                  for Read Capacity Command is prepared
+ * Note:            None
+ *****************************************************************************/	
+	
+void MSDReadCapacityHandler(void)
+{
+/* To be refined with capacity reading */
+	buf[0]=0x02;
+	buf[1]=0x54;
+	buf[2]=0x29;
+	buf[3]=0x80;
+	buf[4]=0x00;
+	buf[5]=0x00;
+	buf[6]=0x02;
+	buf[7]=0x00;
+
+	msd_csw.dCSWDataResidue=0x08;		  // size of response
+	msd_csw.bCSWStatus=0x00;			  // success
+}
+
+/******************************************************************************
+ * Function:        void MSDReadHandler(void)
+ *
+ * PreCondition:    None 
+ *
+ * Input:           None
+ *
+ * Output:          None
+ *
+ * Side Effects:    None
+ *
+ * Overview:        Decodes the CBWCB of READ(10) command to calculate
+ *                  the starting LBA and the Transfer length
+ *                  (number of blocks to be read). Reads a block of 512B data
+ *                  from Mass Storage in buf (by calling SectorRead).
+ *                  If successfully read, the data is sent to the
+ *                  host in 64B chunks (MSD_IN_EP_SIZE) (see MSDDataIn()).
+ *                  This is repeated for TransferLength number of blocks.
+ *                  In case of error bCSWStatus is set to 0x01 and sense data
+ *                  with sense key NOT READY and appropriate ASC,
+ *                  ASCQ codes is prepared.
+ *
+ * Note:            None
+ *****************************************************************************/	
+	
+void MSDReadHandler(void)
+{
+	MSA_Error status;
+	WORD TransferLength;
+	DWORD LBA;
+	byte Flags;
+	
+	LBA.v[3]=gblCBW.CBWCB[2];
+	LBA.v[2]=gblCBW.CBWCB[3];
+	LBA.v[1]=gblCBW.CBWCB[4];
+	LBA.v[0]=gblCBW.CBWCB[5];
+	
+	TransferLength.v[1]=gblCBW.CBWCB[7];
+	TransferLength.v[0]=gblCBW.CBWCB[8];
+	
+	Flags=gblCBW.CBWCB[1];
+	
+	msd_csw.bCSWStatus=0x0;
+	msd_csw.dCSWDataResidue=0x0;
+		
+	while (TransferLength._word > 0) {							
+		TransferLength._word--;					// we have read 1 LBA
+		status=msaValid;
+		if (ata_read_sectors(LBA._dword, 1, (byte*)&buf[0]) == 0)
+			status=msaValid;
+		else
+			status=msaCardBadCmd;
+		LBA._dword++;							// read the next LBA
+		if (status==msaValid) {
+			msd_csw.bCSWStatus=0x00;			// success
+			msd_csw.dCSWDataResidue=BLOCKLEN_512;//in order to send the
+                                                //512 bytes of data read
+			ptrNextData=(byte *)&buf[0];
+			while (msd_csw.dCSWDataResidue>0)
+				MSDDataIn();					// send the data
+			msd_csw.dCSWDataResidue=0x0;		// for next time
+		} else {
+			msd_csw.bCSWStatus=0x01;			// Error 0x01 Refer page#18
+                                                // of BOT specifications
+			/* Don't read any more data*/
+			msd_csw.dCSWDataResidue=0x0;
+			break;	                            // break the loop
+		}
+	}
+}
+
+
+/******************************************************************************
+ * Function:        void MSDDataOut(void)
+ *
+ * PreCondition:    None 
+ *
+ * Input:           None
+ *
+ * Output:          None
+ *
+ * Side Effects:    MSD_BD_OUT.ADR is incremented by MSD_OUT_EP_SIZE
+ *                 (to read next 64B into buf)
+ *
+ * Overview:        This function reads 64B (MSD_OUT_EP_SIZE)
+ *                  from EP1 OUT MSD_BD_OUT
+ * Note:            None
+ *****************************************************************************/	
+	
+void MSDDataOut(void) 
+{
+/* To be done */
+}
+
+
+/******************************************************************************
+ * Function:        void MSDWriteHandler()
+ *
+ * PreCondition:    None 
+ *
+ * Input:           None
+ *
+ * Output:          None
+ *
+ * Side Effects:    None
+ *
+ * Overview:        Decodes the CBWCB of WRITE(10) command to calculate
+ *                  the starting LBA and theTransfer length (number of
+ *                  blocks to be written). Reads TransferLength blocks
+ *                  of data, 1 block=512B at a time in buf.
+ *                  The data from the host, 64B in MSD_BD_OUT, is received
+ *                  in the buf (see MSDDataOut()).
+ *		    The MSD_BD_OUT.ADR pointer is manipulated to fill the 512B
+ *                  buf and when full the data is written to the SD Card 
+ *                  by calling the function SectorWrite(...) (see sdcard.c)
+ *                  In case of error bCSWStatus is set to 0x01 and sense
+ *                  data with sense key NOT READY and appropriate ASC,
+ *                  ASCQ codes is prepared.
+ *
+ * Note:            None
+ *****************************************************************************/	
+ void MSDWriteHandler(void)
+{
+	MSA_Error status=msaValid;
+	WORD TransferLength;
+	DWORD LBA;
+
+ 	/* Read the LBA, TransferLength fields from Command Block
+       NOTE: CB is Big-Endian */
+
+	LBA.v[3]=gblCBW.CBWCB[2];
+	LBA.v[2]=gblCBW.CBWCB[3];
+	LBA.v[1]=gblCBW.CBWCB[4];
+	LBA.v[0]=gblCBW.CBWCB[5];
+	TransferLength.v[1]=gblCBW.CBWCB[7];
+	TransferLength.v[0]=gblCBW.CBWCB[8];
+
+	msd_csw.bCSWStatus=0x0;	
+	while (TransferLength._word > 0) {	
+		msd_csw.dCSWDataResidue=BLOCKLEN_512;
+		/* Read 512B into buf*/
+		while (msd_csw.dCSWDataResidue>0) 
+			MSDDataOut();
+#if 0
+/* To be done */
+		status = SectorWrite((LBA._dword), (byte*)&buf[0]);
+#endif
+		if (status) {
+			msd_csw.bCSWStatus=0x01;
+			/* add some sense keys here*/
+		}
+		LBA._dword++;				// One LBA is written. Write the next LBA
+		TransferLength._word--;
+
+		/* Point MSD_BD_OUT to msd_cbw after
+           done reading the WRITE data from host*/
+#if 0
+/* To be done */
+		if (TransferLength._word>0){
+			MSD_BD_OUT.Cnt=MSD_OUT_EP_SIZE;
+			MSD_BD_OUT.ADR=(byte*)&buf[0];
+		} else {
+			MSD_BD_OUT.Cnt=sizeof(msd_cbw);
+    		MSD_BD_OUT.ADR=(byte*)&msd_cbw;
+    	}
+#endif
+	}	// end of while
+	return;
+}
+
+
+
+/******************************************************************************
+ * Function:        void MSDRequestSenseHandler(void)
+ *
+ * PreCondition:    None 
+ *
+ * Input:           None
+ *
+ * Output:          None
+ *
+ * Side Effects:    None
+ *
+ * Overview:        This function prepares the Sense Data in response
+ *                  to the Request Sense Command The contents of structure
+ *                  RequestSenseResponse are copied to buf and a
+ *                  success bCSWStatus=0x00 is set.
+ *
+ * Note:            None
+ *****************************************************************************/	
+void MSDRequestSenseHandler(void)
+{
+	byte i;
+	for(i=0;i<sizeof(RequestSenseResponse);i++)
+		buf[i]=gblSenseData._byte[i];
+	
+	msd_csw.dCSWDataResidue=sizeof(RequestSenseResponse);
+	msd_csw.bCSWStatus=0x0;					// success
+	return;
+}
+
+/******************************************************************************
+ * Function:        void MSDModeSenseHandler()
+ *
+ * PreCondition:    None 
+ *
+ * Input:           None
+ *
+ * Output:          None
+ *
+ * Side Effects:    None
+ *
+ * Overview:        This function prepares response to the Mode Sense command
+ *                  a basic response is implemented in this version of the code
+ *                  00h imples no other mode pages and 0x03 is the size of the 
+ *                  data (in bytes) that follows.
+ *
+ * Note:            None
+ *****************************************************************************/	
+void MSDModeSenseHandler(void)
+{
+	
+	buf[0]=0x03;
+	buf[1]=0x00;
+	buf[2]=0x00;
+	buf[3]=0x00;
+	
+	msd_csw.bCSWStatus=0x0;
+	msd_csw.dCSWDataResidue=0x04;
+	return;
+}
+
+
+/******************************************************************************
+ * Function:        void MSDMediumRemovalHandler()
+ *
+ * PreCondition:    None 
+ *
+ * Input:           None
+ *
+ * Output:          None
+ *
+ * Side Effects:    None
+ *
+ * Overview:        This function prepares response to Prevent Allow
+ *                  Medium Removal Command No data response is expect only 
+ *                  a CSW with command execution status is expected
+ *                  Since we cannot control the removal of media,
+ *                  we respond by a Success CSW
+ * Note:            None
+ *****************************************************************************/	
+void MSDMediumRemovalHandler(void)
+{
+	msd_csw.bCSWStatus=0x00;
+	msd_csw.dCSWDataResidue=0x00;
+	return;
+}  
+
+/******************************************************************************
+ * Function:        void MSDTestUnitReadyHandler()
+ *
+ * PreCondition:    None 
+ *
+ * Input:           None
+ *
+ * Output:          None
+ *
+ * Side Effects:    None
+ *
+ * Overview:        This function prepares response to Test Unit Ready Command
+ *                  No data response is expected, only a CSW is to be sent 
+ *                  Based on the current state of the SDCard an error or
+ *                  success status value is set
+ *
+ * Note:            None
+ *****************************************************************************/	
+void MSDTestUnitReadyHandler(void)
+{
+	msd_csw.bCSWStatus=0x0;
+	ResetSenseData();
+	msd_csw.dCSWDataResidue=0x00;
+	//SendCSW();
+	return;
+}
+
+/******************************************************************************
+ * Function:        void MSDVerifyHandler()
+ *
+ * PreCondition:    None 
+ *
+ * Input:           None
+ *
+ * Output:          None
+ *
+ * Side Effects:    None
+ *
+ * Overview:        This function prepares response to Verify Command
+ *		    No data response is expected, we reply by a success CSW 
+ *		    The command is not being processed in this version of code
+ *
+ * Note:            None
+ *****************************************************************************/	
+void MSDVerifyHandler(void)
+{
+	msd_csw.bCSWStatus=0x0;
+	msd_csw.dCSWDataResidue=0x00;
+	return;
+}
+
+/******************************************************************************
+ * Function:        void MSDStopStartHandler()
+ *
+ * PreCondition:    None 
+ *
+ * Input:           None
+ *
+ * Output:          None
+ *
+ * Side Effects:    None
+ *
+ * Overview:        This function prepares response to Start Stop Unit Command
+ *                  No data response is expected, we reply by a success CSW 
+ *                  The command is not being processed in this version of code
+ *
+ * Note:            None
+ *****************************************************************************/	
+void MSDStopStartHandler(void)
+{
+	msd_csw.bCSWStatus=0x0;
+	msd_csw.dCSWDataResidue=0x00;
+	return;
+}
+
+/******************************************************************************
+ * Function:        void MSDCommandHandler(void)
+ *
+ * PreCondition:    None 
+ *
+ * Input:           None
+ *
+ * Output:          None
+ *
+ * Side Effects:    None
+ *
+ * Overview: when the MSD_State = MSD_WAIT. This function decodes the CBW
+ *                  Command and takes appropriate action.If the CBW command is
+ *                  not supported the Sense Data is set, CSW status
+ *                  is set to Command Failed (bCSWStatus=01h)
+ *
+ * Note:            None
+ *****************************************************************************/	
+	
+void MSDCommandHandler(void)		// In reality it is to read from EP1
+{	
+	switch(gblCBW.CBWCB[0]) {
+    	case INQUIRY:
+        	MSDInquiryHandler();
+        break;
+        case READ_FORMAT_CAPACITY:
+		    MSDReadFormatCapacityHandler();            
+        break;
+        case READ_CAPACITY:
+		    MSDReadCapacityHandler();            
+        break;
+		case READ_10:
+        	MSDReadHandler();
+	    break;
+    	case WRITE_10:
+    	   	MSDWriteHandler();
+		break;
+        case REQUEST_SENSE:
+		   	MSDRequestSenseHandler();
+        break;
+	    case MODE_SENSE:
+	    	MSDModeSenseHandler();
+    	break;
+		case PREVENT_ALLOW_MEDIUM_REMOVAL:
+		   	MSDMediumRemovalHandler();
+        break;
+		case TEST_UNIT_READY:
+		   	MSDTestUnitReadyHandler();
+        break;
+		case VERIFY:
+		   	MSDVerifyHandler();
+        break;
+		case STOP_START:
+		   	MSDStopStartHandler();
+        break;
+		default:
+        	ResetSenseData();
+			gblSenseData.SenseKey=S_ILLEGAL_REQUEST;
+			gblSenseData.ASC=ASC_INVALID_COMMAND_OPCODE;
+			gblSenseData.ASCQ=ASCQ_INVALID_COMMAND_OPCODE;
+			msd_csw.bCSWStatus=0x01;
+			msd_csw.dCSWDataResidue=0x00;
+ 		break;
+	} // end switch	
+	
+	ptrNextData=(byte*)&buf[0];
+	while ((MSD_State==MSD_DATA_IN) || (MSD_State==MSD_DATA_OUT)) {
+		command_exec(NULL, NULL);
+	}
+}
+
+static int command_exec(struct usb_ep* ep, char* req)
+{
+	byte i;
+	if (MSD_State==MSD_DATA_IN) {
+		/* Send Data to Host */	
+		if(gblCBW.dCBWDataTransferLength==0) 
+		{
+			/* Finished sending the data send the Status */
+			/* SendCSW() send the csw and sets the state to wait */
+			SendCSW(); 
+		}	
+		else 
+		{ 				
+			/* Still have data to Send */
+			MSDDataIn();
+		}
+		return 0;	
+	}	
+	if (MSD_State==MSD_DATA_OUT) {			
+		/* Receive data from Host*/
+		if(gblCBW.dCBWDataTransferLength==0) { 	
+			/* Finished receiving the data prepare and send the status */
+		  	if ((msd_csw.bCSWStatus==0x00)&&(msd_csw.dCSWDataResidue!=0)) 
+		  		msd_csw.bCSWStatus=0x02;
+			SendCSW();	// sends the csw and sets the state to wait
+		}
+		/*
+		* Note that MSD_DATA_OUT State is reached only for the WRITE_10 COMMAND
+		* Also note that this code is reached in MSD_DATA_OUT State only after
+		*  we have read the required amount of data from the host
+		* This procsssing is done in WriteCommandHandler because we have
+		* limited buffer space. We read from host in 64Bytes chunks
+		* (size of MSD_BD_OUT), fill the buf(512B) and write the
+        *  data into the SDCard
+		*/ 
+		return 0;
+	}
+	if (MSD_State==MSD_WAIT) {
+		/* Copy the received cbw into the gblCBW */
+		msd_cbw = (USB_MSD_CBW *) req;
+		gblCBW.dCBWSignature=msd_cbw->dCBWSignature;					
+		gblCBW.dCBWTag=msd_cbw->dCBWTag;
+		gblCBW.dCBWDataTransferLength=msd_cbw->dCBWDataTransferLength;
+    	gblCBW.bCBWFlags=msd_cbw->bCBWFlags;
+    	gblCBW.bCBWLUN=msd_cbw->bCBWLUN;
+	    gblCBW.bCBWCBLength=msd_cbw->bCBWCBLength;
+    	for (i=0;i<msd_cbw->bCBWCBLength;i++)
+    		gblCBW.CBWCB[i]=msd_cbw->CBWCB[i];
+	    gblCBWLength=sizeof(USB_MSD_CBW);
+		if (IsValidCBW()) {
+			if (IsMeaningfulCBW()) {
+				PrepareCSWData();
+				/* If direction is device to host*/
+				if (gblCBW.bCBWFlags==0x80)
+					MSD_State=MSD_DATA_IN;
+				else if (gblCBW.bCBWFlags==0x00) {
+					/* If direction is host to device*/
+					/* prepare to read data in buf */
+					res.length = BUFFER_SIZE;
+					res.buf = buf;
+					ops->receive(ep, &res);
+    				MSD_State=MSD_DATA_OUT;
+				}
+				/* Decode and process the valid and meaningful CBW received */
+				MSDCommandHandler();									
+			}
+			/* NOTE:
+			 * In case when the received CBW is not valid or meaningful,
+			* one can take action  such as Stall the EP1 and go through reset
+			 * recovery or turn on error LED etc.
+			 */ 
+		}
+	}
+	return 0;
+}
+