void usb_getStringDescriptor(usb_device_t* device)
{
#ifdef _USB_TRANSFER_DIAGNOSIS_
textColor(LIGHT_CYAN);
printf("\n\nUSB: GET_DESCRIPTOR string, dev: %X endpoint: 0 languageIDs", device);
textColor(TEXT);
#endif
struct usb_stringDescriptor descriptor;
usb_transfer_t transfer;
usb_setupTransfer(device->disk->port, &transfer, USB_CONTROL, 0, 64);
usb_setupTransaction(&transfer, 8, 0x80, 6, 3, 0, 0, 12);
usb_inTransaction(&transfer, false, &descriptor, 12);
usb_outTransaction(&transfer, true, 0, 0);
usb_issueTransfer(&transfer);
#ifdef _USB_TRANSFER_DIAGNOSIS_
memshow(&descriptor, 12, false);
putch('\n');
#endif
showStringDescriptor(&descriptor);
}
void usb_getUnicodeStringDescriptor(usb_device_t* device, uint32_t stringIndex)
{
#ifdef _USB_TRANSFER_DIAGNOSIS_
textColor(LIGHT_CYAN);
printf("\n\nUSB: GET_DESCRIPTOR string, dev: %X endpoint: 0 stringIndex: %u", device, stringIndex);
textColor(TEXT);
#endif
char buffer[64];
usb_transfer_t transfer;
usb_setupTransfer(device->disk->port, &transfer, USB_CONTROL, 0, 64);
usb_setupTransaction(&transfer, 8, 0x80, 6, 3, stringIndex, 0x0409, 64);
usb_inTransaction(&transfer, false, buffer, 64);
usb_outTransaction(&transfer, true, 0, 0);
usb_issueTransfer(&transfer);
#ifdef _USB_TRANSFER_DIAGNOSIS_
memshow(buffer, 64, false);
putch('\n');
#endif
showUnicodeStringDescriptor((struct usb_stringDescriptorUnicode*)buffer, device, stringIndex);
}
bool usb_getConfigDescriptor(usb_device_t* device)
{
#ifdef _USB_TRANSFER_DIAGNOSIS_
textColor(LIGHT_CYAN);
printf("\n\nUSB: GET_DESCRIPTOR Config");
textColor(TEXT);
#endif
char buffer[32];
usb_transfer_t transfer;
usb_setupTransfer(device->disk->port, &transfer, USB_CONTROL, 0, 64);
usb_setupTransaction(&transfer, 8, 0x80, 6, 2, 0, 0, 32);
usb_inTransaction(&transfer, false, buffer, 32);
usb_outTransaction(&transfer, true, 0, 0);
usb_issueTransfer(&transfer);
if (transfer.success)
{
#ifdef _USB_TRANSFER_DIAGNOSIS_
textColor(LIGHT_GRAY);
printf("\n---------------------------------------------------------------------\n");
textColor(GREEN);
#endif
// parse to config (len=9,type=2), interface (len=9,type=4) or endpoint (len=7,type=5)
void* addr = buffer;
void* lastByte = addr + (*(uint16_t*)(addr+2)); // totalLength (WORD)
#ifdef _USB_DIAGNOSIS_
memshow(buffer, *(uint16_t*)(addr+2), false);
putch('\n');
#endif
while ((uintptr_t)addr < (uintptr_t)lastByte)
{
uint8_t type = *(uint8_t*)(addr+1);
uint8_t length = *(uint8_t*)addr;
if (length == 9 && type == 2)
{
struct usb_configurationDescriptor* descriptor = addr;
showConfigurationDescriptor(descriptor);
}
else if (length == 9 && type == 4)
{
struct usb_interfaceDescriptor* descriptor = addr;
showInterfaceDescriptor(descriptor);
if (descriptor->interfaceClass == 8)
{
// store interface number for mass storage transfers
device->numInterfaceMSD = descriptor->interfaceNumber;
device->InterfaceClass = descriptor->interfaceClass;
device->InterfaceSubclass = descriptor->interfaceSubclass;
}
}
else if (length == 7 && type == 5)
{
struct usb_endpointDescriptor* descriptor = addr;
showEndpointDescriptor(descriptor);
if((descriptor->endpointAddress & 0xF) < 3)
device->endpoints[descriptor->endpointAddress & 0xF].mps = descriptor->maxPacketSize;
// store endpoint numbers for IN/OUT mass storage transfers, attributes must be 0x2, because there are also endpoints with attributes 0x3(interrupt)
if (descriptor->endpointAddress & 0x80 && descriptor->attributes == 0x2)
{
device->numEndpointInMSD = descriptor->endpointAddress & 0xF;
}
if (!(descriptor->endpointAddress & 0x80) && descriptor->attributes == 0x2)
{
device->numEndpointOutMSD = descriptor->endpointAddress & 0xF;
}
}
else
{
#ifdef _USB_TRANSFER_DIAGNOSIS_
printf("\nlength: %u type: %u - unknown\n", length, type);
#endif
}
addr += length;
}
}
return (transfer.success);
}
int main(int argc, char *argv[])
{
int i, j, flag, len = DEFAULT_LEN;
unsigned int memaddr = 0;
volatile void * mappedaddr = NULL;
unsigned char cmd[MAX_CMD_LEN];
unsigned char cmd_temp[MAX_CMD_LEN];
FILE * cmd_fd = NULL;
unsigned char * cmd_data = NULL;
unsigned char * cmd1_data = NULL;
unsigned char * cmd2_data = NULL;
unsigned int showoffset, showlen, writeoffset;
unsigned char writedata = 0;
if(argc < 2){
printf("Usage: ./memtool addr\n");
exit(1);
}
util_init();
// cpld_version();
memaddr = strtoul(argv[1], NULL, 16);
printf("\nYou input address: 0x%x\n", memaddr);
mappedaddr = mmap(0, MAP_LEN, PROT_READ | PROT_WRITE, MAP_SHARED, util_fd, memaddr & ~MAP_MASK);
if(mappedaddr == (void *) -1){
perror("Error mapping memory");
close(util_fd);
return -1;
}
printf(" | ");
for(i = 0; i < 16; i ++){
printf("%02x ", i);
}
printf("\n-----------------------------------------------------");
flag = 0;
for(i = 1, j = 0; i <= len; i ++){
if(flag == 0){
printf("\n[%02x]| ", j);
j += 1;
flag = 1;
}
printf("%02x ", *((unsigned char *)mappedaddr+i-1));
if(i%16 == 0){
flag = 0;
}
}
printf("\n");
cmd_fd = fopen("/dev/console", "r");
if( cmd_fd == NULL){
perror("Console open");
exit(1);
}
for(;;){
printf("\nPlease input a command, like: d 0x100 0x10\n:");
if(NULL == fgets(cmd, MAX_CMD_LEN, cmd_fd)){
printf("Please input a command.\n");
continue;
}
memset(cmd_temp, 0, MAX_CMD_LEN);
switch(cmd[0]){
case 'd':
case 'D':
cmd_data = strchr(cmd, ' ');
if(cmd_data == NULL){
printf("Please input a valid command.\n");
continue;
}
cmd_data ++;
cmd1_data = strchr(cmd_data, ' ');
if(cmd1_data != NULL){
memcpy(cmd_temp, cmd_data, cmd1_data-cmd_data);
}else{
printf("Please input valid command.\n");
continue;
}
showoffset = strtoul(cmd_temp, NULL, 16);
if(showoffset + showlen >= MAP_LEN){
showoffset = 0;
showlen = DEFAULT_LEN;
printf("The offset should less than 0x%x\n", MAP_LEN);
}
showlen = strtoul(cmd1_data, NULL, 16);
if(showlen == 0) showlen = DEFAULT_LEN;
printf("\nAddr: %p, len: 0x%x\n", memaddr+showoffset, showlen);
memshow(mappedaddr+showoffset, showlen);
break;
case 'e':
case 'E':
cmd_data = strchr(cmd, ' ');
cmd_data ++;
writeoffset = strtoul(cmd_data, NULL, 16);
if(writeoffset >= MAP_LEN){
writeoffset = 0;
printf("The offset should less than 0x%x\n", MAP_LEN);
}
for(i = 0; ; i ++){
memset(cmd, 0, MAX_CMD_LEN);
printf("%p[%02x]:", memaddr+writeoffset+i, *(volatile unsigned char *)(mappedaddr+writeoffset+i));
if(NULL == fgets(cmd, MAX_CMD_LEN, cmd_fd)){
printf("Please input a command.\n");
continue;
}
// printf("cmd: %s, 0:%x\n", cmd, cmd[0]);
if(cmd[0] == '\n'){
// printf("Aborted\n");
break;
}
for(j = 0; ; j ++){
if(cmd[j] == '\n') break;
if(isxdigit(cmd[j]) == 0){
printf("Please input a valid hex data.");
break;
}
}
if(cmd[j] != '\n') break;
writedata = strtoul(cmd, NULL, 16);
// printf("We write 0x%x to %p\n", writedata, memaddr+writeoffset+i);
*(volatile unsigned char *)(mappedaddr+writeoffset+i) = writedata;
}
break;
case 'q':
case 'Q':
i = munmap((void *)mappedaddr, MAP_LEN);
if(i == -1){
printf("error unmapping memory\n");
return -1;
}
util_exit();
fclose(cmd_fd);
exit(0);
break;
default:
printf("Please input a command, like: d 0x100 0x10\n");
continue;
}
}
}
/// cf. http://www.beyondlogic.org/usbnutshell/usb4.htm#Bulk
void usb_sendSCSICommand(usb_device_t* device, uint32_t interface, uint32_t endpointOut, uint32_t endpointIn, uint8_t SCSIcommand, uint32_t LBA, uint16_t TransferLength, void* dataBuffer, void* statusBuffer)
{
#ifdef _USB_DIAGNOSIS_
printf("\nOUT part");
textColor(0x03);
printf("\ntoggle OUT %u", device->ToggleEndpointOutMSD);
textColor(TEXT);
#endif
struct usb_CommandBlockWrapper cbw;
formatSCSICommand(SCSIcommand, &cbw, LBA, TransferLength);
usb_transfer_t transfer;
usb_setupTransfer(device->disk->port, &transfer, USB_BULK, endpointOut, 512);
usb_outTransaction(&transfer, false, &cbw, 31);
usb_issueTransfer(&transfer);
if (SCSIcommand == 0x28 || SCSIcommand == 0x2A) // read(10) and write(10)
{
TransferLength *= 512; // byte = 512 * block
}
/**************************************************************************************************************************************/
#ifdef _USB_DIAGNOSIS_
printf("\nIN part");
#endif
uint32_t timeout = 0;
char tempStatusBuffer[13];
if(statusBuffer == 0)
statusBuffer = tempStatusBuffer;
usb_setupTransfer(device->disk->port, &transfer, USB_BULK, endpointIn, 512);
if (TransferLength > 0)
{
usb_inTransaction(&transfer, false, dataBuffer, TransferLength);
usb_inTransaction(&transfer, false, statusBuffer, 13);
}
else
{
usb_inTransaction(&transfer, false, statusBuffer, 13);
}
usb_issueTransfer(&transfer);
#ifdef _USB_DIAGNOSIS_
if (TransferLength) // byte
{
putch('\n');
memshow(dataBuffer, TransferLength, false);
putch('\n');
if ((TransferLength==512) || (TransferLength==36)) // data block (512 byte), inquiry feedback (36 byte)
{
memshow(dataBuffer, TransferLength, true); // alphanumeric
putch('\n');
}
}
#endif
if(checkSCSICommand(statusBuffer, device, TransferLength, SCSIcommand) != 0 && timeout < 5)
{
timeout++;
}
// TODO: Handle failure/timeout
}
static int checkSCSICommand(void* MSDStatus, usb_device_t* device, uint16_t TransferLength, uint8_t SCSIOpcode)
{
// CSW Status
#ifdef _USB_DIAGNOSIS_
putch('\n');
memshow(MSDStatus,13, false);
putch('\n');
#endif
int error = 0;
// check signature 0x53425355 // DWORD 0 (byte 0:3)
uint32_t CSWsignature = *(uint32_t*)MSDStatus; // DWORD 0
if (CSWsignature == CSWMagicOK)
{
#ifdef _USB_DIAGNOSIS_
textColor(SUCCESS);
printf("\nCSW signature OK ");
textColor(TEXT);
#endif
}
else if (CSWsignature == CSWMagicNotOK)
{
textColor(ERROR);
printf("\nCSW signature wrong (not processed)");
textColor(TEXT);
return -1;
}
else
{
textColor(ERROR);
printf("\nCSW signature wrong (processed, but wrong value)");
textColor(TEXT);
error = -2;
}
// check matching tag
uint32_t CSWtag = *(((uint32_t*)MSDStatus)+1); // DWORD 1 (byte 4:7)
if ((BYTE1(CSWtag) == SCSIOpcode) && (BYTE2(CSWtag) == 0x42) && (BYTE3(CSWtag) == 0x42) && (BYTE4(CSWtag) == 0x42))
{
#ifdef _USB_DIAGNOSIS_
textColor(SUCCESS);
printf("CSW tag %yh OK ",BYTE1(CSWtag));
textColor(TEXT);
#endif
}
else
{
textColor(ERROR);
printf("\nError: CSW tag wrong");
textColor(TEXT);
error = -3;
}
// check CSWDataResidue
uint32_t CSWDataResidue = *(((uint32_t*)MSDStatus)+2); // DWORD 2 (byte 8:11)
if (CSWDataResidue == 0)
{
#ifdef _USB_DIAGNOSIS_
textColor(SUCCESS);
printf("\tCSW data residue OK ");
textColor(TEXT);
#endif
}
else
{
textColor(0x06);
printf("\nCSW data residue: %u", CSWDataResidue);
textColor(TEXT);
}
// check status byte // DWORD 3 (byte 12)
uint8_t CSWstatusByte = *(((uint8_t*)MSDStatus)+12); // byte 12 (last byte of 13 bytes)
textColor(ERROR);
switch (CSWstatusByte)
{
case 0x00:
#ifdef _USB_DIAGNOSIS_
textColor(SUCCESS);
printf("\tCSW status OK");
#endif
break;
case 0x01:
printf("\nCommand failed");
error = -4;
break;
case 0x02:
printf("\nPhase Error");
textColor(IMPORTANT);
printf("\nReset recovery is needed");
usb_resetRecoveryMSD(device, device->numInterfaceMSD);
error = -5;
break;
default:
printf("\nCSW status byte: undefined value (error)");
error = -6;
break;
}
textColor(TEXT);
return error;
}
