berlin_tools/bootloader/common/drivers/usb_ctrl/dwc2/usb_mass.c - manifest_repos/bootloader - Git at Google

 /*
  * Copyright (C) 2018 Synaptics Incorporated. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  *
  * INFORMATION CONTAINED IN THIS DOCUMENT IS PROVIDED "AS-IS," AND
  * SYNAPTICS EXPRESSLY DISCLAIMS ALL EXPRESS AND IMPLIED WARRANTIES,
  * INCLUDING ANY IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  * A PARTICULAR PURPOSE, AND ANY WARRANTIES OF NON-INFRINGEMENT OF ANY
  * INTELLECTUAL PROPERTY RIGHTS. IN NO EVENT SHALL SYNAPTICS BE LIABLE
  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, PUNITIVE, OR
  * CONSEQUENTIAL DAMAGES ARISING OUT OF OR IN CONNECTION WITH THE USE
  * OF THE INFORMATION CONTAINED IN THIS DOCUMENT, HOWEVER CAUSED AND
  * BASED ON ANY THEORY OF LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
  * NEGLIGENCE OR OTHER TORTIOUS ACTION, AND EVEN IF SYNAPTICS WAS
  * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. IF A TRIBUNAL OF
  * COMPETENT JURISDICTION DOES NOT PERMIT THE DISCLAIMER OF DIRECT
  * DAMAGES OR ANY OTHER DAMAGES, SYNAPTICS' TOTAL CUMULATIVE LIABILITY
  * TO ANY PARTY SHALL NOT EXCEED ONE HUNDRED U.S. DOLLARS.
  */
 #include "diag_common.h"
 #include "diag_misc.h"
 #include "Galois_memmap.h"
 #include "soc.h"
 #include "diag_gic.h"
 #include "diag_cpu.h"
 #include "diag_misc.h"
 #include "global.h"
 #include "usb_memmap.h"
 #include "usb_typedefs.h"
 #include "usb_mass_ufi.h"
 #include "core.h"

 static struct usb_mass_data usb_stor = {0};
 static int g_udev_bulk_in_epnum = 1; // Mass Storage EP assignment
 static int g_udev_bulk_out_epnum = 2; // Mass Storage EP assignment
 static char *gp_mass_cdb_buff = (char *)USB_MASS_BULK_CDB_BASE;
 static char *gp_mass_csw_buff = (char *)USB_MASS_BULK_CSW_BASE;
 static char *gp_mass_data_buff = (char *)USB_MASS_BULK_DATA_BASE;
 static int g_mass_lun = 0; // LUN for MASS Storage
 static int g_mass_tag = 0;
 static int g_mass_last_lba = 0; //Mass Storage last lba
 static int g_usb_init = 0; //Indicate if usb is initialized in mass storage
 int g_mass_blen = 512; // Mass Storage block len
 int g_mass_blocking = 0;
 int g_mass_storage_timeout = 0; // 1000 = 1 second, 0: no timeout
 unsigned int g_usbTrCompleteCount = 0;
 int g_usb_mass_isr_state = MASS_WAIT_NEW_CMD;
 int g_usb_mass_data_dir;
 int g_usb_mass_data_len;
 char *g_usb_mass_data;

 static void mymemcpy(char *d, char *s, int count)
 {
 	while(count){
 		*d++ = *s++;
 		count--;
 	}
 }

 static unsigned int usb_mass_getmaxlun(int *maxlun)
 {
     struct usb_mass_data *us = &usb_stor;
     int len;
     int ret;
     unsigned char *result = USB_memalloc(1);
     len = urb_control(us->pusb_dev,
                   usb_rcvctrlpipe(us->pusb_dev, 0),
                   US_BBB_GET_MAX_LUN,
                   USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_IN,
                   0, us->ifnum,
                   result, sizeof(char),
                   USB_CNTL_TIMEOUT * 5);
     dbg_printf(PRN_RES,"Get Max LUN -> len = %d, result = %d\n", len, (int) *result);

 	g_mass_tag = 1; // Start Mass Storage CDW tag to 1
     if (len > 0)  {
         *maxlun = *result;
         ret = 0;
     }else {
         *maxlun = 0;
         ret = -EINVAL;
     }

     USB_memfree(result);
     return ret;
 }

 static int usb_mass_data_queue()
 {
 	int res;
 	unsigned long pipe_bulk_in, pipe_bulk_out;

 	if (g_usb_mass_data_dir == USB_DIR_IN){
 		pipe_bulk_in = usb_rcvbulkpipe(pdev_usb, g_udev_bulk_in_epnum);
 		res = que_blk_msg(pdev_usb, pipe_bulk_in, g_usb_mass_data, g_usb_mass_data_len);
 	}
 	else{
 		pipe_bulk_out = usb_sndbulkpipe(pdev_usb, g_udev_bulk_out_epnum);
 		res = que_blk_msg(pdev_usb, pipe_bulk_out, g_usb_mass_data, g_usb_mass_data_len);
 	}

 	RETURN_ON_ERROR(res)
 	return 0;
 }

 static int usb_mass_status_queue()
 {
 	unsigned long pipe_bulk_in;
 	int res;

 	pipe_bulk_in = usb_rcvbulkpipe(pdev_usb, g_udev_bulk_in_epnum);
 	res = que_blk_msg(pdev_usb, pipe_bulk_in, gp_mass_csw_buff, CSW_SIZE);

 	RETURN_ON_ERROR(res)

 	return 0;
 }

 void usb_mass_callback()
 {

 	switch(g_usb_mass_isr_state){
 		case MASS_WAIT_CMD_DONE:
 		//Queue Data
 			dbg_disp("DATA PHASE\n");
 			usb_mass_data_queue();
 			g_usb_mass_isr_state = MASS_WAIT_DATA_DONE;
 			break;
 		case MASS_WAIT_DATA_DONE:
 		//Queue Status
 			dbg_disp("STATUS PHASE\n");
 			usb_mass_status_queue();
 			g_usb_mass_isr_state = MASS_WAIT_STATUS_DONE;
 			break;
 		case MASS_WAIT_STATUS_DONE:
 			dbg_disp("COMPLETE PHASE\n");
 			g_usbTrCompleteCount++; // Let MI know.
 			g_usb_mass_isr_state = MASS_WAIT_NEW_CMD;
 			break;
 	}
 }


 static int usb_mass_generic_queue(char *cdb, char *data, int data_len, int dir)
 {
 	unsigned long pipe_bulk_out;
 	int res;

 	// Build pipe
 	pipe_bulk_out = usb_sndbulkpipe(pdev_usb, g_udev_bulk_out_epnum);

 	g_usb_mass_data_dir = dir;
 	g_usb_mass_data_len = data_len;
 	g_usb_mass_data = data;

 	mymemcpy(gp_mass_cdb_buff, cdb, CBW_SIZE);
 	dbg_dump_buffer((unsigned char *)gp_mass_cdb_buff,31);

 	g_usb_mass_isr_state = data_len ? MASS_WAIT_CMD_DONE:MASS_WAIT_DATA_DONE;

 	dbg_disp("COMMAND PHASE\n");
 	res = que_blk_msg(pdev_usb, pipe_bulk_out, gp_mass_cdb_buff, CBW_SIZE);
 	RETURN_ON_ERROR(res)

 	return 0;
 }

 static int usbmass_generic(char *cdb, char *data, int data_len, int dir)
 {
 	unsigned long pipe_bulk_in, pipe_bulk_out;
 	int res;

 	if (g_mass_blocking)
 	{
 		// Build pipe
 		pipe_bulk_in = usb_rcvbulkpipe(pdev_usb, g_udev_bulk_in_epnum);
 		pipe_bulk_out = usb_sndbulkpipe(pdev_usb, g_udev_bulk_out_epnum);

 		//Command Phase
 		mymemcpy(gp_mass_cdb_buff, cdb, CBW_SIZE);
 		dbg_out("CDB[");dbg_outdw((unsigned long)gp_mass_cdb_buff);dbg_out("]");
 		dbg_dump_buffer((unsigned char *)gp_mass_cdb_buff,31);
 		res = snd_blk_msg(pdev_usb, pipe_bulk_out, gp_mass_cdb_buff, CBW_SIZE);
 		RETURN_ON_ERROR(res)

 		//Data Phase if data_len>0
 		if (data_len){
 			if (dir == USB_DIR_IN){
 				res = snd_blk_msg(pdev_usb, pipe_bulk_in, data, data_len);
 				RETURN_ON_ERROR(res)
 				dbg_out("POST DATA IN[");dbg_outdw((unsigned long)data);dbg_out("]");
 				dbg_dump_buffer((unsigned char *)data,min(data_len,8));
 				if (data_len>=16){
 					dbg_out("POST DATA IN[");dbg_outdw((unsigned long)(data+data_len-8));dbg_out("]");
 					dbg_dump_buffer((unsigned char *)(data+data_len-8),8);
 				}
 			}
 			else{
 				dbg_out("DATA_OUT["); dbg_outdw((unsigned long)data); dbg_out(",");dbg_outdw((unsigned int)data_len);
 				dbg_dump_buffer((unsigned char *)data,min(data_len,16));
 				res = snd_blk_msg(pdev_usb, pipe_bulk_out, data, data_len);
 				RETURN_ON_ERROR(res)
 			}
 		}


 		//Status phase
 		res = snd_blk_msg(pdev_usb, pipe_bulk_in, gp_mass_csw_buff, CSW_SIZE);
 		RETURN_ON_ERROR(res)
 		dbg_out("POST CSW[");dbg_outdw((unsigned long)gp_mass_csw_buff); dbg_out("]");
 		dbg_dump_buffer((unsigned char*)gp_mass_csw_buff,CSW_SIZE);
 	}
 	else{
 		unsigned int tr_compl_count_pre = g_usbTrCompleteCount;
 		int timeout=g_mass_storage_timeout;

 		usb_mass_generic_queue( cdb, data, data_len, dir);

 		while(g_usbTrCompleteCount==tr_compl_count_pre){
 			// To Do. Implement some kind of Time out here in case of error.
 			dbg_printf(PRN_INFO,".");
 			mdelay(1);
 			if ((!timeout) && g_mass_storage_timeout ){
 				dbg_printf(PRN_RES,"ERR: Mass Storage Timeout!\n");
 				return 1;
 			}
 			if (g_mass_storage_timeout)
 				timeout--;
 		}
 	}
 	return 0;
 }

 int usb_mass_ufi_generic
 (
 /* [IN] command object allocated by application*/
 uint_8                     opcode,
 uint_8                     lun,
 uint_32                    lbaddr,
 uint_32                    blen,

 uint_8                     cbwflags,

 uchar_ptr                  buf,
 uint_32                    buf_len
 )
 { /* Body */
 	//int status = USB_OK;
 	CBW_STRUCT *cbw_ptr = (CBW_STRUCT *)gp_mass_cdb_buff;
 	UFI_CBWCB_EXTENDED  *ufi_ptr = (UFI_CBWCB_EXTENDED *)cbw_ptr->CBWCB;

 	memset(gp_mass_cdb_buff, 0, CBW_SIZE);

 	/* Construct UFI command buffer */
 	ufi_ptr->BUFIOPCODE = opcode;
 	ufi_ptr->BUFILUN = lun;
 	utou32swap(ufi_ptr->BUFILOGICALBLOCKADDRESS, lbaddr);
 	utou32swap(ufi_ptr->BLENGTH, blen);

 	/* Construct CBW fields (sig and tag will be filled up by class driver)*/
 	utou32(cbw_ptr->DCBWDATALENGTH, buf_len);
 	cbw_ptr->BMCBWFLAGS = cbwflags;
 	transfer_low_nibble(g_mass_lun, cbw_ptr->BCBWCBLUN);
 	transfer_low_nibble(sizeof(UFI_CBWCB_EXTENDED),
 		cbw_ptr->BCBWCBLENGTH);
 	utou32(cbw_ptr->DCBWSIGNATURE, CBW_SIGNATURE);
 	utou32(cbw_ptr->DCBWTAG, g_mass_tag++); // increment mass storage command tag here

 	return usbmass_generic((char*) cbw_ptr, (char *) buf, buf_len, cbwflags);

 } /* Endbody */

 void usb_mass_parse_capacity(char *b, int *last , int *len)
 {
 	unsigned long last_lba, blk_len;

 	last_lba = (((unsigned long)b[0]) << 24) |
 		(((unsigned long)b[1]) << 16) |
 		(((unsigned long)b[2]) << 8) |
 		(((unsigned long)b[3]) & 0x0FF);
 	blk_len = (((unsigned long)b[4]) << 24) |
 		(((unsigned long)b[5]) << 16) |
 		(((unsigned long)b[6]) << 8) |
 		(((unsigned long)b[7]) & 0x0FF);

 	*last = last_lba;
 	*len = blk_len;

 	dbg_printf(PRN_RES,"USB MASS STORAGE INFO: LAST_LBA=0x%08lX BLK_LEN=%d bytes\n", last_lba, blk_len);
 }

 int usb_mass_probe(void)
 {
     struct usb_interface *iface;
     int i;
     struct usb_endpoint_descriptor *ep_desc;
     //unsigned int flags = 0;
     struct usb_device *dev = pdev_usb;
     struct usb_mass_data *ss = &usb_stor;
     unsigned int ifnum = 0;

     /* let's examine the device now */
     iface = &dev->config.if_desc[ifnum];

     if (dev->descriptor.bDeviceClass != 0 ||
             iface->desc.bInterfaceClass != USB_CLASS_MASS_STORAGE ||
             iface->desc.bInterfaceSubClass < US_SC_MIN ||
             iface->desc.bInterfaceSubClass > US_SC_MAX) {
         debug("Not mass storage\n");
         /* if it's not a mass storage, we go no further */
         return 0;
     }

     memset(ss, 0, sizeof(struct usb_mass_data));

     /* At this point, we know we've got a live one */
     debug("\n\nUSB Mass Storage device detected\n");

     /* Initialize the us_data structure with some useful info */
     ss->ifnum = ifnum;
     ss->pusb_dev = dev;

     /*
      * We are expecting a minimum of 2 endpoints - in and out (bulk).
      * An optional interrupt is OK (necessary for CBI protocol).
      * We will ignore any others.
      */
     for (i = 0; i < iface->desc.bNumEndpoints; i++) {
         ep_desc = &iface->ep_desc[i];
         /* is it an BULK endpoint? */
         if ((ep_desc->bmAttributes &
              USB_ENDPOINT_XFERTYPE_MASK) == USB_ENDPOINT_XFER_BULK) {
             if (ep_desc->bEndpointAddress & USB_DIR_IN)
                 ss->ep_in = ep_desc->bEndpointAddress &
                         USB_ENDPOINT_NUMBER_MASK;
             else
                 ss->ep_out =
                     ep_desc->bEndpointAddress &
                     USB_ENDPOINT_NUMBER_MASK;
         }

         /* is it an interrupt endpoint? */
         if ((ep_desc->bmAttributes &
              USB_ENDPOINT_XFERTYPE_MASK) == USB_ENDPOINT_XFER_INT) {
             ss->ep_int = ep_desc->bEndpointAddress &
                         USB_ENDPOINT_NUMBER_MASK;
             ss->irqinterval = ep_desc->bInterval;
         }
     }
     debug("Endpoints In %d Out %d Int %d\n",
           ss->ep_in, ss->ep_out, ss->ep_int);
     g_udev_bulk_in_epnum = ss->ep_in;
     g_udev_bulk_out_epnum = ss->ep_out;

     return set_interface(dev, iface->desc.bInterfaceNumber, 0);
 }

 int usb_mass_start()
 {
 	int res;
 	static int mass_started = 0;

 	if (!mass_started){
 		mass_started = 1;
 		// Dynamically allocate
 		gp_mass_cdb_buff = USB_memalloc(64);
 		gp_mass_csw_buff = USB_memalloc(64);
 		gp_mass_data_buff = USB_memalloc(USB_MASS_BULK_DATA_SIZE);	//2MB
 	}

 	dbg_printf(PRN_RES,"\t--SET_INTERFACE..");
     res = usb_mass_probe();
 	RETURN_ON_ERROR(res);
 	dbg_printf(PRN_RES,"OK\n");

 	dbg_printf(PRN_RES,"\t--GET_LUN..");
 	res = usb_mass_getmaxlun(&g_mass_lun);
 	RETURN_ON_ERROR(res);
 	dbg_printf(PRN_RES,"got:%d .. OK\n",g_mass_lun);

 	//	TEST UNIT READY
 	dbg_printf(PRN_RES,"\t--TEST_UNIT_READY..");
 	res = usb_mass_ufi_test_unit_ready();
 	RETURN_ON_ERROR(res);
 	dbg_printf(PRN_RES,"OK\n");
 	USB_MASS_SENSE

 	// READ CAPACITY to get last_lba and blk_len, before we do any READ /WRITE block based.
 	dbg_printf(PRN_RES,"\t--READ_CAPACITY..");
 	res = usb_mass_ufi_read_capacity((uchar_ptr)gp_mass_data_buff, sizeof(MASS_STORAGE_READ_CAPACITY_CMD_STRUCT_INFO));
 	RETURN_ON_ERROR(res);
 	dbg_printf(PRN_RES,"OK\n");
 	//now parse the result to extract important mass storage information.
 	usb_mass_parse_capacity(gp_mass_data_buff, &g_mass_last_lba, &g_mass_blen);
 	USB_MASS_SENSE

 	//	READ10
 	memset(gp_mass_data_buff,0,g_mass_blen); // SET read buffer to 0
 	dbg_printf(PRN_RES,"\t--READ10..");
 	res = usb_mass_ufi_read_10( 8, (uchar_ptr) gp_mass_data_buff, g_mass_blen , 1);
 	RETURN_ON_ERROR(res);
 	dbg_printf(PRN_RES,"OK\n");

 	//	WRITE10
 	dbg_printf(PRN_RES,"\t--WRITE10..");
 #if PLATFORM == VELOCE	//we don't use File System in mass storage
 	// Initialize input buffer.
 	{ int i;
 	unsigned char *pb= (uchar_ptr)gp_mass_data_buff;
 	for(i=0;i<g_mass_blen;i++) *pb++=i&0x0ff;
 	}

 	res = usb_mass_ufi_write_10(8, (uchar_ptr)gp_mass_data_buff, g_mass_blen, 1);
 	RETURN_ON_ERROR(res);
 	dbg_printf(PRN_RES,"OK\n");
 #else
 	dbg_printf(PRN_RES,"SKIPPED\n");
 #endif

 	//	INQUIRY
 	dbg_printf(PRN_RES,"\t--INQUIRY..");
 	res = usb_mass_ufi_inquiry((uchar_ptr)gp_mass_data_buff, sizeof(INQUIRY_DATA_FORMAT));
 	RETURN_ON_ERROR(res);
 	dbg_printf(PRN_RES,"OK\n");

 	//	PREVENT ALLOW MEDIUM REMOVAL
 	dbg_printf(PRN_RES,"\t--PREVENT MEDIUM REMOVAL..");
 	res = usb_mass_ufi_prevent_allow_medium_removal(1); //prevent
 	RETURN_ON_ERROR(res);
 	dbg_printf(PRN_RES,"OK\n");
 	g_usb_init = 1;

 	return 0;
 }

 int usb_storage_init()
 {
 	int res;

 	if (g_usb_init)
 		return 0;
 	res = usb_mass_start();
 	RETURN_ON_ERROR(res);

 	return 0;
 }

 int usb_storage_read(unsigned char *buff,int lba, int blk_count)
 {
 	int res;
 	int bc,n;

 	res = usb_storage_init();
 	RETURN_ON_ERROR(res);

 	bc = 0;
 	while(bc < blk_count){
 		n = min(256,(blk_count-bc));
 		res = usb_mass_ufi_read_10( lba+bc, (uchar_ptr)buff+bc*g_mass_blen, n* g_mass_blen, n);
 	RETURN_ON_ERROR(res);
 		bc += n;
 	}

 	return 0;
 }

 int usb_storage_write(unsigned char *buff, int lba, int blk_count)
 {
 	int res;
 	int bc,n;

 		bc = 0;
 	res = usb_storage_init();
 	RETURN_ON_ERROR(res);

 	while(bc < blk_count){
 		n = min(256,(blk_count-bc));
 		res = usb_mass_ufi_write_10(lba+bc, (uchar_ptr)buff+bc*g_mass_blen, n* g_mass_blen, n);
 	RETURN_ON_ERROR(res);
 		bc+= n;
 	}

 	return 0;
 }
	/*
	* Copyright (C) 2018 Synaptics Incorporated. All rights reserved.
	*
	* This program is free software; you can redistribute it and/or modify
	* it under the terms of the GNU General Public License version 2 as
	* published by the Free Software Foundation.
	*
	* INFORMATION CONTAINED IN THIS DOCUMENT IS PROVIDED "AS-IS," AND
	* SYNAPTICS EXPRESSLY DISCLAIMS ALL EXPRESS AND IMPLIED WARRANTIES,
	* INCLUDING ANY IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
	* A PARTICULAR PURPOSE, AND ANY WARRANTIES OF NON-INFRINGEMENT OF ANY
	* INTELLECTUAL PROPERTY RIGHTS. IN NO EVENT SHALL SYNAPTICS BE LIABLE
	* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, PUNITIVE, OR
	* CONSEQUENTIAL DAMAGES ARISING OUT OF OR IN CONNECTION WITH THE USE
	* OF THE INFORMATION CONTAINED IN THIS DOCUMENT, HOWEVER CAUSED AND
	* BASED ON ANY THEORY OF LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
	* NEGLIGENCE OR OTHER TORTIOUS ACTION, AND EVEN IF SYNAPTICS WAS
	* ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. IF A TRIBUNAL OF
	* COMPETENT JURISDICTION DOES NOT PERMIT THE DISCLAIMER OF DIRECT
	* DAMAGES OR ANY OTHER DAMAGES, SYNAPTICS' TOTAL CUMULATIVE LIABILITY
	* TO ANY PARTY SHALL NOT EXCEED ONE HUNDRED U.S. DOLLARS.
	*/
	#include "diag_common.h"
	#include "diag_misc.h"
	#include "Galois_memmap.h"
	#include "soc.h"
	#include "diag_gic.h"
	#include "diag_cpu.h"
	#include "diag_misc.h"
	#include "global.h"
	#include "usb_memmap.h"
	#include "usb_typedefs.h"
	#include "usb_mass_ufi.h"
	#include "core.h"

	static struct usb_mass_data usb_stor = {0};
	static int g_udev_bulk_in_epnum = 1; // Mass Storage EP assignment
	static int g_udev_bulk_out_epnum = 2; // Mass Storage EP assignment
	static char gp_mass_cdb_buff = (char )USB_MASS_BULK_CDB_BASE;
	static char gp_mass_csw_buff = (char )USB_MASS_BULK_CSW_BASE;
	static char gp_mass_data_buff = (char )USB_MASS_BULK_DATA_BASE;
	static int g_mass_lun = 0; // LUN for MASS Storage
	static int g_mass_tag = 0;
	static int g_mass_last_lba = 0; //Mass Storage last lba
	static int g_usb_init = 0; //Indicate if usb is initialized in mass storage
	int g_mass_blen = 512; // Mass Storage block len
	int g_mass_blocking = 0;
	int g_mass_storage_timeout = 0; // 1000 = 1 second, 0: no timeout
	unsigned int g_usbTrCompleteCount = 0;
	int g_usb_mass_isr_state = MASS_WAIT_NEW_CMD;
	int g_usb_mass_data_dir;
	int g_usb_mass_data_len;
	char *g_usb_mass_data;

	static void mymemcpy(char d, char s, int count)
	{
	while(count){
	d++ = s++;
	count--;
	}
	}

	static unsigned int usb_mass_getmaxlun(int *maxlun)
	{
	struct usb_mass_data *us = &usb_stor;
	int len;
	int ret;
	unsigned char *result = USB_memalloc(1);
	len = urb_control(us->pusb_dev,
	usb_rcvctrlpipe(us->pusb_dev, 0),
	US_BBB_GET_MAX_LUN,
	USB_TYPE_CLASS \| USB_RECIP_INTERFACE \| USB_DIR_IN,
	0, us->ifnum,
	result, sizeof(char),
	USB_CNTL_TIMEOUT * 5);
	dbg_printf(PRN_RES,"Get Max LUN -> len = %d, result = %d\n", len, (int) *result);

	g_mass_tag = 1; // Start Mass Storage CDW tag to 1
	if (len > 0) {
	maxlun = result;
	ret = 0;
	}else {
	*maxlun = 0;
	ret = -EINVAL;
	}

	USB_memfree(result);
	return ret;
	}

	static int usb_mass_data_queue()
	{
	int res;
	unsigned long pipe_bulk_in, pipe_bulk_out;

	if (g_usb_mass_data_dir == USB_DIR_IN){
	pipe_bulk_in = usb_rcvbulkpipe(pdev_usb, g_udev_bulk_in_epnum);
	res = que_blk_msg(pdev_usb, pipe_bulk_in, g_usb_mass_data, g_usb_mass_data_len);
	}
	else{
	pipe_bulk_out = usb_sndbulkpipe(pdev_usb, g_udev_bulk_out_epnum);
	res = que_blk_msg(pdev_usb, pipe_bulk_out, g_usb_mass_data, g_usb_mass_data_len);
	}

	RETURN_ON_ERROR(res)
	return 0;
	}

	static int usb_mass_status_queue()
	{
	unsigned long pipe_bulk_in;
	int res;

	pipe_bulk_in = usb_rcvbulkpipe(pdev_usb, g_udev_bulk_in_epnum);
	res = que_blk_msg(pdev_usb, pipe_bulk_in, gp_mass_csw_buff, CSW_SIZE);

	RETURN_ON_ERROR(res)

	return 0;
	}

	void usb_mass_callback()
	{

	switch(g_usb_mass_isr_state){
	case MASS_WAIT_CMD_DONE:
	//Queue Data
	dbg_disp("DATA PHASE\n");
	usb_mass_data_queue();
	g_usb_mass_isr_state = MASS_WAIT_DATA_DONE;
	break;
	case MASS_WAIT_DATA_DONE:
	//Queue Status
	dbg_disp("STATUS PHASE\n");
	usb_mass_status_queue();
	g_usb_mass_isr_state = MASS_WAIT_STATUS_DONE;
	break;
	case MASS_WAIT_STATUS_DONE:
	dbg_disp("COMPLETE PHASE\n");
	g_usbTrCompleteCount++; // Let MI know.
	g_usb_mass_isr_state = MASS_WAIT_NEW_CMD;
	break;
	}
	}


	static int usb_mass_generic_queue(char cdb, char data, int data_len, int dir)
	{
	unsigned long pipe_bulk_out;
	int res;

	// Build pipe
	pipe_bulk_out = usb_sndbulkpipe(pdev_usb, g_udev_bulk_out_epnum);

	g_usb_mass_data_dir = dir;
	g_usb_mass_data_len = data_len;
	g_usb_mass_data = data;

	mymemcpy(gp_mass_cdb_buff, cdb, CBW_SIZE);
	dbg_dump_buffer((unsigned char *)gp_mass_cdb_buff,31);

	g_usb_mass_isr_state = data_len ? MASS_WAIT_CMD_DONE:MASS_WAIT_DATA_DONE;

	dbg_disp("COMMAND PHASE\n");
	res = que_blk_msg(pdev_usb, pipe_bulk_out, gp_mass_cdb_buff, CBW_SIZE);
	RETURN_ON_ERROR(res)

	return 0;
	}

	static int usbmass_generic(char cdb, char data, int data_len, int dir)
	{
	unsigned long pipe_bulk_in, pipe_bulk_out;
	int res;

	if (g_mass_blocking)
	{
	// Build pipe
	pipe_bulk_in = usb_rcvbulkpipe(pdev_usb, g_udev_bulk_in_epnum);
	pipe_bulk_out = usb_sndbulkpipe(pdev_usb, g_udev_bulk_out_epnum);

	//Command Phase
	mymemcpy(gp_mass_cdb_buff, cdb, CBW_SIZE);
	dbg_out("CDB[");dbg_outdw((unsigned long)gp_mass_cdb_buff);dbg_out("]");
	dbg_dump_buffer((unsigned char *)gp_mass_cdb_buff,31);
	res = snd_blk_msg(pdev_usb, pipe_bulk_out, gp_mass_cdb_buff, CBW_SIZE);
	RETURN_ON_ERROR(res)

	//Data Phase if data_len>0
	if (data_len){
	if (dir == USB_DIR_IN){
	res = snd_blk_msg(pdev_usb, pipe_bulk_in, data, data_len);
	RETURN_ON_ERROR(res)
	dbg_out("POST DATA IN[");dbg_outdw((unsigned long)data);dbg_out("]");
	dbg_dump_buffer((unsigned char *)data,min(data_len,8));
	if (data_len>=16){
	dbg_out("POST DATA IN[");dbg_outdw((unsigned long)(data+data_len-8));dbg_out("]");
	dbg_dump_buffer((unsigned char *)(data+data_len-8),8);
	}
	}
	else{
	dbg_out("DATA_OUT["); dbg_outdw((unsigned long)data); dbg_out(",");dbg_outdw((unsigned int)data_len);
	dbg_dump_buffer((unsigned char *)data,min(data_len,16));
	res = snd_blk_msg(pdev_usb, pipe_bulk_out, data, data_len);
	RETURN_ON_ERROR(res)
	}
	}


	//Status phase
	res = snd_blk_msg(pdev_usb, pipe_bulk_in, gp_mass_csw_buff, CSW_SIZE);
	RETURN_ON_ERROR(res)
	dbg_out("POST CSW[");dbg_outdw((unsigned long)gp_mass_csw_buff); dbg_out("]");
	dbg_dump_buffer((unsigned char*)gp_mass_csw_buff,CSW_SIZE);
	}
	else{
	unsigned int tr_compl_count_pre = g_usbTrCompleteCount;
	int timeout=g_mass_storage_timeout;

	usb_mass_generic_queue( cdb, data, data_len, dir);

	while(g_usbTrCompleteCount==tr_compl_count_pre){
	// To Do. Implement some kind of Time out here in case of error.
	dbg_printf(PRN_INFO,".");
	mdelay(1);
	if ((!timeout) && g_mass_storage_timeout ){
	dbg_printf(PRN_RES,"ERR: Mass Storage Timeout!\n");
	return 1;
	}
	if (g_mass_storage_timeout)
	timeout--;
	}
	}
	return 0;
	}

	int usb_mass_ufi_generic
	(
	/* [IN] command object allocated by application*/
	uint_8 opcode,
	uint_8 lun,
	uint_32 lbaddr,
	uint_32 blen,

	uint_8 cbwflags,

	uchar_ptr buf,
	uint_32 buf_len
	)
	{ /* Body */
	//int status = USB_OK;
	CBW_STRUCT cbw_ptr = (CBW_STRUCT )gp_mass_cdb_buff;
	UFI_CBWCB_EXTENDED ufi_ptr = (UFI_CBWCB_EXTENDED )cbw_ptr->CBWCB;

	memset(gp_mass_cdb_buff, 0, CBW_SIZE);

	/* Construct UFI command buffer */
	ufi_ptr->BUFIOPCODE = opcode;
	ufi_ptr->BUFILUN = lun;
	utou32swap(ufi_ptr->BUFILOGICALBLOCKADDRESS, lbaddr);
	utou32swap(ufi_ptr->BLENGTH, blen);

	/* Construct CBW fields (sig and tag will be filled up by class driver)*/
	utou32(cbw_ptr->DCBWDATALENGTH, buf_len);
	cbw_ptr->BMCBWFLAGS = cbwflags;
	transfer_low_nibble(g_mass_lun, cbw_ptr->BCBWCBLUN);
	transfer_low_nibble(sizeof(UFI_CBWCB_EXTENDED),
	cbw_ptr->BCBWCBLENGTH);
	utou32(cbw_ptr->DCBWSIGNATURE, CBW_SIGNATURE);
	utou32(cbw_ptr->DCBWTAG, g_mass_tag++); // increment mass storage command tag here

	return usbmass_generic((char) cbw_ptr, (char ) buf, buf_len, cbwflags);

	} /* Endbody */

	void usb_mass_parse_capacity(char b, int last , int *len)
	{
	unsigned long last_lba, blk_len;

	last_lba = (((unsigned long)b[0]) << 24) \|
	(((unsigned long)b[1]) << 16) \|
	(((unsigned long)b[2]) << 8) \|
	(((unsigned long)b[3]) & 0x0FF);
	blk_len = (((unsigned long)b[4]) << 24) \|
	(((unsigned long)b[5]) << 16) \|
	(((unsigned long)b[6]) << 8) \|
	(((unsigned long)b[7]) & 0x0FF);

	*last = last_lba;
	*len = blk_len;

	dbg_printf(PRN_RES,"USB MASS STORAGE INFO: LAST_LBA=0x%08lX BLK_LEN=%d bytes\n", last_lba, blk_len);
	}

	int usb_mass_probe(void)
	{
	struct usb_interface *iface;
	int i;
	struct usb_endpoint_descriptor *ep_desc;
	//unsigned int flags = 0;
	struct usb_device *dev = pdev_usb;
	struct usb_mass_data *ss = &usb_stor;
	unsigned int ifnum = 0;

	/* let's examine the device now */
	iface = &dev->config.if_desc[ifnum];

	if (dev->descriptor.bDeviceClass != 0 \|\|
	iface->desc.bInterfaceClass != USB_CLASS_MASS_STORAGE \|\|
	iface->desc.bInterfaceSubClass < US_SC_MIN \|\|
	iface->desc.bInterfaceSubClass > US_SC_MAX) {
	debug("Not mass storage\n");
	/* if it's not a mass storage, we go no further */
	return 0;
	}

	memset(ss, 0, sizeof(struct usb_mass_data));

	/* At this point, we know we've got a live one */
	debug("\n\nUSB Mass Storage device detected\n");

	/* Initialize the us_data structure with some useful info */
	ss->ifnum = ifnum;
	ss->pusb_dev = dev;

	/*
	* We are expecting a minimum of 2 endpoints - in and out (bulk).
	* An optional interrupt is OK (necessary for CBI protocol).
	* We will ignore any others.
	*/
	for (i = 0; i < iface->desc.bNumEndpoints; i++) {
	ep_desc = &iface->ep_desc[i];
	/* is it an BULK endpoint? */
	if ((ep_desc->bmAttributes &
	USB_ENDPOINT_XFERTYPE_MASK) == USB_ENDPOINT_XFER_BULK) {
	if (ep_desc->bEndpointAddress & USB_DIR_IN)
	ss->ep_in = ep_desc->bEndpointAddress &
	USB_ENDPOINT_NUMBER_MASK;
	else
	ss->ep_out =
	ep_desc->bEndpointAddress &
	USB_ENDPOINT_NUMBER_MASK;
	}

	/* is it an interrupt endpoint? */
	if ((ep_desc->bmAttributes &
	USB_ENDPOINT_XFERTYPE_MASK) == USB_ENDPOINT_XFER_INT) {
	ss->ep_int = ep_desc->bEndpointAddress &
	USB_ENDPOINT_NUMBER_MASK;
	ss->irqinterval = ep_desc->bInterval;
	}
	}
	debug("Endpoints In %d Out %d Int %d\n",
	ss->ep_in, ss->ep_out, ss->ep_int);
	g_udev_bulk_in_epnum = ss->ep_in;
	g_udev_bulk_out_epnum = ss->ep_out;

	return set_interface(dev, iface->desc.bInterfaceNumber, 0);
	}

	int usb_mass_start()
	{
	int res;
	static int mass_started = 0;

	if (!mass_started){
	mass_started = 1;
	// Dynamically allocate
	gp_mass_cdb_buff = USB_memalloc(64);
	gp_mass_csw_buff = USB_memalloc(64);
	gp_mass_data_buff = USB_memalloc(USB_MASS_BULK_DATA_SIZE); //2MB
	}

	dbg_printf(PRN_RES,"\t--SET_INTERFACE..");
	res = usb_mass_probe();
	RETURN_ON_ERROR(res);
	dbg_printf(PRN_RES,"OK\n");

	dbg_printf(PRN_RES,"\t--GET_LUN..");
	res = usb_mass_getmaxlun(&g_mass_lun);
	RETURN_ON_ERROR(res);
	dbg_printf(PRN_RES,"got:%d .. OK\n",g_mass_lun);

	// TEST UNIT READY
	dbg_printf(PRN_RES,"\t--TEST_UNIT_READY..");
	res = usb_mass_ufi_test_unit_ready();
	RETURN_ON_ERROR(res);
	dbg_printf(PRN_RES,"OK\n");
	USB_MASS_SENSE

	// READ CAPACITY to get last_lba and blk_len, before we do any READ /WRITE block based.
	dbg_printf(PRN_RES,"\t--READ_CAPACITY..");
	res = usb_mass_ufi_read_capacity((uchar_ptr)gp_mass_data_buff, sizeof(MASS_STORAGE_READ_CAPACITY_CMD_STRUCT_INFO));
	RETURN_ON_ERROR(res);
	dbg_printf(PRN_RES,"OK\n");
	//now parse the result to extract important mass storage information.
	usb_mass_parse_capacity(gp_mass_data_buff, &g_mass_last_lba, &g_mass_blen);
	USB_MASS_SENSE

	// READ10
	memset(gp_mass_data_buff,0,g_mass_blen); // SET read buffer to 0
	dbg_printf(PRN_RES,"\t--READ10..");
	res = usb_mass_ufi_read_10( 8, (uchar_ptr) gp_mass_data_buff, g_mass_blen , 1);
	RETURN_ON_ERROR(res);
	dbg_printf(PRN_RES,"OK\n");

	// WRITE10
	dbg_printf(PRN_RES,"\t--WRITE10..");
	#if PLATFORM == VELOCE //we don't use File System in mass storage
	// Initialize input buffer.
	{ int i;
	unsigned char *pb= (uchar_ptr)gp_mass_data_buff;
	for(i=0;i<g_mass_blen;i++) *pb++=i&0x0ff;
	}

	res = usb_mass_ufi_write_10(8, (uchar_ptr)gp_mass_data_buff, g_mass_blen, 1);
	RETURN_ON_ERROR(res);
	dbg_printf(PRN_RES,"OK\n");
	#else
	dbg_printf(PRN_RES,"SKIPPED\n");
	#endif

	// INQUIRY
	dbg_printf(PRN_RES,"\t--INQUIRY..");
	res = usb_mass_ufi_inquiry((uchar_ptr)gp_mass_data_buff, sizeof(INQUIRY_DATA_FORMAT));
	RETURN_ON_ERROR(res);
	dbg_printf(PRN_RES,"OK\n");

	// PREVENT ALLOW MEDIUM REMOVAL
	dbg_printf(PRN_RES,"\t--PREVENT MEDIUM REMOVAL..");
	res = usb_mass_ufi_prevent_allow_medium_removal(1); //prevent
	RETURN_ON_ERROR(res);
	dbg_printf(PRN_RES,"OK\n");
	g_usb_init = 1;

	return 0;
	}

	int usb_storage_init()
	{
	int res;

	if (g_usb_init)
	return 0;
	res = usb_mass_start();
	RETURN_ON_ERROR(res);

	return 0;
	}

	int usb_storage_read(unsigned char *buff,int lba, int blk_count)
	{
	int res;
	int bc,n;

	res = usb_storage_init();
	RETURN_ON_ERROR(res);

	bc = 0;
	while(bc < blk_count){
	n = min(256,(blk_count-bc));
	res = usb_mass_ufi_read_10( lba+bc, (uchar_ptr)buff+bcg_mass_blen, n g_mass_blen, n);
	RETURN_ON_ERROR(res);
	bc += n;
	}

	return 0;
	}

	int usb_storage_write(unsigned char *buff, int lba, int blk_count)
	{
	int res;
	int bc,n;

	bc = 0;
	res = usb_storage_init();
	RETURN_ON_ERROR(res);

	while(bc < blk_count){
	n = min(256,(blk_count-bc));
	res = usb_mass_ufi_write_10(lba+bc, (uchar_ptr)buff+bcg_mass_blen, n g_mass_blen, n);
	RETURN_ON_ERROR(res);
	bc+= n;
	}

	return 0;
	}