ufi.c

网上收集的利用nios软核

#include "common.h"
#include "UFI.h"
extern USB_BULK_FSM bulkFsmStatus;
INQUIRY_DATA inquiryData;
CAPACITY_DATA capacityData;
char *disk;
void UFIDataInit(void)
{
/* Inquiry data*/
	inquiryData.peripheralDeviceType = 0x00;
	inquiryData.RMB = 0x80;
	inquiryData.version = 0x0;
	inquiryData.responseDataFormat = 0x01;
	inquiryData.additionalLength = 0x1f;
	memset(inquiryData.reserved,0,3);
	//memcpy(inquiryData.vendorInformation,"zsw",sizeof("zsw"));
	// Vendor Information
	inquiryData.vendorInformation[0] = 'S';
	inquiryData.vendorInformation[1] = 'J';
	inquiryData.vendorInformation[2] = 'T';
	inquiryData.vendorInformation[3] = 'U';
	inquiryData.vendorInformation[4] = '\'';
	inquiryData.vendorInformation[5] = 'E';
	inquiryData.vendorInformation[6] = 'E';
	inquiryData.vendorInformation[7] = ' ';
	// Produce Identification
	memcpy(inquiryData.productIdentification,"zsw'USB Flash   ",sizeof("zsw'USB Flash   "));
	// Product RevisionLevel
	memcpy(inquiryData.productRevisionLevel,"Disk",4);
/**/
/*Disk Space*/
	disk = malloc(DISK_SIZE*sizeof(char));
	
}


int UFI_Inquiry(char LUN)
{
	printf("Inquiry data length: %d\n",sizeof(INQUIRY_DATA));	
	debugShowBuf(&( inquiryData.peripheralDeviceType),sizeof(INQUIRY_DATA));
	printf("inquiry data address0x%08x\n",&inquiryData);
	usb_1362_epx_transmit(EP1, (char *)&( inquiryData.peripheralDeviceType),sizeof(INQUIRY_DATA));
	bulkFsmStatus.usbBulkFSMStatus = USB_BULK_CSW;
	bulkFsmStatus.CSWStatus = CSW_GOOD;

}
int UFI_ReadCapacity(char LUN)
{
	int blockSizeInBytes;
	int sizeOfDisk;
	int sizeOfDiskInBlock;	
	sizeOfDisk = DISK_SIZE;
	blockSizeInBytes = BLOCK_SIZE;
	sizeOfDiskInBlock = DISK_SIZE/BLOCK_SIZE - 1;

	if(LUN == 0){
		//memcpy(&(capacityData.BlockLengthInBytes), (char *)&blockSizeInBytes,4);
		//memcpy(&(capacityData.lastLogicalBlockAddress),(char *)&blockSizeInBytes,4);
		//
		capacityData.BlockLengthInBytes[3] = ((char *)&blockSizeInBytes)[0];
		capacityData.BlockLengthInBytes[2] = ((char *)&blockSizeInBytes)[1];
		capacityData.BlockLengthInBytes[1] = ((char *)&blockSizeInBytes)[2];
		capacityData.BlockLengthInBytes[0] = ((char *)&blockSizeInBytes)[3];
		//
		capacityData.lastLogicalBlockAddress[3] = ((char *)&sizeOfDiskInBlock)[0];
		capacityData.lastLogicalBlockAddress[2] = ((char *)&sizeOfDiskInBlock)[1];
		capacityData.lastLogicalBlockAddress[1] = ((char *)&sizeOfDiskInBlock)[2];
		capacityData.lastLogicalBlockAddress[0] = ((char *)&sizeOfDiskInBlock)[3];
		usb_1362_epx_transmit(EP1,(char *)&capacityData,sizeof(CAPACITY_DATA));
		}
	bulkFsmStatus.usbBulkFSMStatus = USB_BULK_CSW;
	bulkFsmStatus.CSWStatus = CSW_GOOD;
		
		
}
int UFI_Read10(USB_BULK_FSM *bulkFsmStatus)
{
	char *addr;
	int readSize;
	int LBA;// Logic Block Address
	int blockSize;
	char buffer[64];
/*Get LBA*/
	memcpy((char *)&LBA + 0,bulkFsmStatus->CBWbuf + 20,1);
	memcpy((char *)&LBA + 1,bulkFsmStatus->CBWbuf + 19,1);
	memcpy((char *)&LBA + 2,bulkFsmStatus->CBWbuf + 18,1);
	memcpy((char *)&LBA + 3,bulkFsmStatus->CBWbuf + 17,1);
/*Get Block Size*/
	blockSize = ((0x0ff&bulkFsmStatus->CBWbuf[22])<<8) +
			0x0ff & (bulkFsmStatus->CBWbuf[23]);
/*Save the LBA and LBLength*/
	bulkFsmStatus->UFI_LBA = LBA;
	bulkFsmStatus->UFI_LBLength = blockSize;
	bulkFsmStatus->bulkDataTransferLength = blockSize * BLOCK_SIZE;
printf("LBA:%d\n",LBA);
printf("LBLength:%d\n",blockSize);
	if(bulkFsmStatus->bulkDataTransferLength > EP1_BUFFER_SIZE){
		usb_1362_epx_transmit(EP1,disk+LBA*BLOCK_SIZE,EP1_BUFFER_SIZE);
		bulkFsmStatus->bulkDataTransferAddress = disk+LBA*BLOCK_SIZE + EP1_BUFFER_SIZE;
		bulkFsmStatus->bulkDataTransferLength -= EP1_BUFFER_SIZE;
		bulkFsmStatus->usbBulkFSMStatus = USB_BULK_DATA_IN;
		}
	else{
		usb_1362_epx_transmit(EP1,disk+LBA*BLOCK_SIZE,bulkFsmStatus->bulkDataTransferLength);
		bulkFsmStatus->bulkDataTransferLength -= bulkFsmStatus->bulkDataTransferLength;
		bulkFsmStatus->usbBulkFSMStatus = USB_BULK_CSW;
		bulkFsmStatus->CSWStatus = CSW_GOOD;
		
		}
		
	
}
int UFI_ModeSense(USB_BULK_FSM *bulkFsmStatus)
{
	char pageCode;
	char pc;
	short parameterListLength;
	UFI_READ_WRITE_RECOVERY_PAGE readWriteRecoveryPage;
/*Get UFI cmd info*/
	pageCode = 0x03f & bulkFsmStatus->CBWbuf[17];
	pc = (0xc0 & bulkFsmStatus->CBWbuf[17])>>6;

	((char *)&parameterListLength)[0] = bulkFsmStatus->CBWbuf[23];
	((char *)&parameterListLength)[1] = bulkFsmStatus->CBWbuf[22];
/*Prepare readWriteRecoveryPage Data*/
	readWriteRecoveryPage.PS_Pagecode = 0x01;
	readWriteRecoveryPage.pageLength = 0x0a;
	readWriteRecoveryPage.errorRecoveryParameters = 0x0;
	readWriteRecoveryPage.readRetryCount = 0x03;
	memset(readWriteRecoveryPage.reserved1,0,4);
	readWriteRecoveryPage.writeRetryCount = 0x03;
	memset(readWriteRecoveryPage.reserved2,0,3);

	if(pageCode == 0x3f){
	// all page
		usb_1362_epx_transmit(EP1,&readWriteRecoveryPage,sizeof(UFI_READ_WRITE_RECOVERY_PAGE));
		bulkFsmStatus->usbBulkFSMStatus = USB_BULK_CSW;
		bulkFsmStatus->CSWStatus = CSW_GOOD;}
	else if(pageCode == 0x1c){
		usb_1362_epx_transmit(EP1,&readWriteRecoveryPage,sizeof(UFI_READ_WRITE_RECOVERY_PAGE));
		bulkFsmStatus->usbBulkFSMStatus = USB_BULK_CSW;
		bulkFsmStatus->CSWStatus = CSW_GOOD;}
		
	

}
int UFI_TestUnitReady(USB_BULK_FSM *bulkFsmStatus)
{
	//usb_1362_epx_transmit(EP1,0,0);
	bulkFsmStatus->usbBulkFSMStatus = USB_BULK_CSW;
	bulkFsmStatus->CSWStatus = CSW_GOOD;
	usbBulkCSWHandler(bulkFsmStatus);
	

}
int UFI_PreventAllowMediumRemoval(USB_BULK_FSM *bulkFsmStatus)
{
	bulkFsmStatus->usbBulkFSMStatus = USB_BULK_CSW;
	bulkFsmStatus->CSWStatus = CSW_GOOD;
	usbBulkCSWHandler(bulkFsmStatus);

}
int UFI_Write10(USB_BULK_FSM *bulkFsmStatus)
{
	unsigned short blockLength;
	int LBA;
	char * startAddress;
/**/

	blockLength = ((0x0ff & bulkFsmStatus->CBWbuf[22])<<8) + 0x0ff & (bulkFsmStatus->CBWbuf[23]);
	((char *)&LBA)[0] = bulkFsmStatus->CBWbuf[20];
	((char *)&LBA)[1] = bulkFsmStatus->CBWbuf[19];	
	((char *)&LBA)[2] = bulkFsmStatus->CBWbuf[18];
	((char *)&LBA)[3] = bulkFsmStatus->CBWbuf[17];
/**/
uartprintf("LBA: %d\n",LBA);
uartprintf("length: %d\n",blockLength);
	startAddress = disk + LBA * BLOCK_SIZE;
/**/
	bulkFsmStatus->usbBulkFSMStatus = USB_BULK_DATA_OUT;
	bulkFsmStatus->bulkDataTransferAddress = startAddress;
	bulkFsmStatus->bulkDataTransferLength = blockLength * BLOCK_SIZE;
	//bulkFsmStatus->bulkDataOutCount = bulkFsmStatus->bulkDataTransferLength / EP1_BUFFER_SIZE + 1;

	
	
}
int UFI_Handler(USB_BULK_FSM *bulkFsmStatus)
{
	char UFICmd;
	UFICmd = 0x0ff &  bulkFsmStatus->CBWbuf[15];
/*Data Phase*/

	switch(UFICmd){
		case 0x12:
			printf("UFI Inquiry\n");
			uartPrintf("UFI Inquiry\n");
			UFI_Inquiry(0);
			break;
		//case 0x23:
		//	printf("UFI ")
		case 0x25:
			printf("UFI Read Capacity\n");
			uartPrintf("UFI Read Capacity\n");
			UFI_ReadCapacity(0);
			break;
		case 0x28:
			printf("UFI Read 10\n");	
			uartPrintf("UFI Read 10\n");	
			UFI_Read10(bulkFsmStatus);
			break;
		case 0x00:
			printf("UFI Test Unit Ready\n");
			uartPrintf("UFI Test Unit Ready\n");
			UFI_TestUnitReady(bulkFsmStatus);
			break;
		case 0x5a:
			printf("UFI Mode Sense\n");
			uartPrintf("UFI Mode Sense\n");			
			UFI_ModeSense(bulkFsmStatus);
            		break;
		case 0x1e:
			printf("UFI Prevent Allow Medium Removal\n");
			uartPrintf("UFI Prevent Allow Medium Removal\n");		
			UFI_PreventAllowMediumRemoval(bulkFsmStatus);
			break;
		case 0x2a:
			printf("UFI write10\n");
			uartprintf("UFI write10\n");
			uartPrintf("UFI write10\n");
			UFI_Write10(bulkFsmStatus);
			break;
		default:
			uartprintf("Unsupport UFI CMD:%x\n",UFICmd);
			bulkFsmStatus->usbBulkFSMStatus = USB_BULK_STALL;
			//bulkFsmStatus->usbBulkFSMStatus = USB_BULK_IDLE;

			break;
		}
/*CSW or Stall Phase*/
	if(bulkFsmStatus->usbBulkFSMStatus == USB_BULK_STALL){
		//usbBulkStall();
		printf("STALL!\n");
		//usb_1362_epx_stall(EP2);
		usleep(10000);
		usb_1362_epx_stall(EP1);
		usleep(1000);
		usb_1362_epx_unstall(EP1);
		//usb_1362_epx_unstall(EP2);
		//usbBulkUnstall();
		bulkFsmStatus->CSWStatus = CSW_BAD;
		//usb_1362_epx_transmit(EP1,0,0);
		usbBulkCSWHandler(bulkFsmStatus);
	}
	/**/
}