USBHost_t36 USB Mass Storage Driver Experiments

Is this a powered hub by any chance?

No, it is a Belkin four port HUB that I have been using for quite some time now and works with MSC. Have never had an issue with it. It has a power LED that lets me know when VUSB has been turned on.
 
@wwaton It was a bug that crept in. Fixed and working.

Did a pull on the library and results were this:

With HUB:
Code:
Start.

SWI_IRQ_NUM 30


USB HOST READY.
No media. Waiting to mount /



Configuring: parent = 0, port = 1, speed = 2
$$$$$$$$$$$$$$$$$$$$$$$$$ ADDRESS 1: 0 retries.
$$$$$$$$$$$$$$$$$$$$$$$$$ ADDRESS 1: 0 retries.
USBHub: checking numep 1, klass 09, interface.klass 09, protocol 00/01-00?01
USBHub: checking numep 1, klass 09, interface.klass 09, protocol 00/01-01?01
USBHub Accepting address assignment 01
dispatchPkt status code 08
dispatchPkt OTHER
USBHub configure 01 04 02



Configuring: parent = 1, port = 4, speed = 2
$$$$$$$$$$$$$$$$$$$$$$$$$ ADDRESS 2: 0 retries.
$$$$$$$$$$$$$$$$$$$$$$$$$ ADDRESS 2: 0 retries.
BulkOnly: checking numep 2, klass 00, subklass 00
BulkOnly: checking protocol 00, interface.klass 08, interface.subklass 06
BulkOnly: checking interface.protocol 50
BulkOnly: checking numep 2, klass 00, subklass 00
BulkOnly: checking protocol 00, interface.klass 08, interface.subklass 06
BulkOnly: checking interface.protocol 50

BS SetInterface
ep: 0x00 bmAttributes: 0x02 index: 1

ep: 0x01 bmAttributes: 0x02 index: 2

BS Start, speed: 2
BS Start
GetMaxLUN 0x00
MaxLUN 0
Inquiry 0x24 0x00
LUN 0 `KingstonDataTraveler 3.0'
Qualifier 0 Device type 00 RMB 1 SSCS 0 SCSI version 06
Device conforms to T10/1731-D (SPC-4) standards.
CheckLUN...
9
>>>>>>>>>>>>>>>>CAPACITY OK ON LUN 0
039a33f300000200

9
Checked LUN...
Onstart begin
BS configured

[COLOR="#FF0000"]Configuring returned 00[/COLOR]

Without HUB:

Code:
Start.

SWI_IRQ_NUM 30


USB HOST READY.
No media. Waiting to mount /



Configuring: parent = 0, port = 1, speed = 2
$$$$$$$$$$$$$$$$$$$$$$$$$ ADDRESS 1: 0 retries.
$$$$$$$$$$$$$$$$$$$$$$$$$ ADDRESS 1: 0 retries.
USBHub: checking numep 2, klass 00, interface.klass 08, protocol 50/00-00?01
BulkOnly: checking numep 2, klass 00, subklass 00
BulkOnly: checking protocol 00, interface.klass 08, interface.subklass 06
BulkOnly: checking interface.protocol 50
USBHub: checking numep 2, klass 00, interface.klass 08, protocol 50/00-01?01
BulkOnly: checking numep 2, klass 00, subklass 00
BulkOnly: checking protocol 00, interface.klass 08, interface.subklass 06
BulkOnly: checking interface.protocol 50
BS SetInterface
ep: 0x00 bmAttributes: 0x02 index: 1

ep: 0x01 bmAttributes: 0x02 index: 2

BS Start, speed: 2
BS Start
GetMaxLUN 0x00
MaxLUN 0
Inquiry 0x24 0x00
LUN 0 `KingstonDataTraveler 3.0'
Qualifier 0 Device type 00 RMB 1 SSCS 0 SCSI version 06
Device conforms to T10/1731-D (SPC-4) standards.
CheckLUN...
7
>>>>>>>>>>>>>>>>CAPACITY OK ON LUN 0
039a33f300000200

7
Checked LUN...
Onstart begin
BS configured

I also tried compiling "UHS_FS_NEW_DEMO.ino" in "UHS_KINETIS_FS_HOST" and got this error:

Code:
Generating function prototypes...
/home/wwatson/arduino-1.8.15/hardware/teensy/../tools/arm/bin/arm-none-eabi-g++ -E -CC -x c++ -w -g -Wall -ffunction-sections -fdata-sections -nostdlib -mno-unaligned-access -fno-exceptions -fpermissive -felide-constructors -std=gnu++14 -Wno-error=narrowing -fno-rtti -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 -fsingle-precision-constant -D__MK66FX1M0__ -DTEENSYDUINO=154 -DARDUINO=10815 -DARDUINO_TEENSY36 -DF_CPU=180000000 -DUSB_SERIAL -DLAYOUT_US_ENGLISH -I/home/wwatson/arduino-1.8.15/hardware/teensy/avr/cores/teensy3 -I/home/wwatson/Arduino/libraries/RTClib -I/home/wwatson/Arduino/libraries/UHS_host -I/home/wwatson/Arduino/libraries/ISR_safe_memory -I/home/wwatson/arduino-1.8.15/hardware/teensy/avr/libraries/Wire -I/home/wwatson/arduino-1.8.15/hardware/teensy/avr/libraries/SPI -I/home/wwatson/Arduino/libraries/UHS_ByteBuffer -I/home/wwatson/Arduino/libraries/UHS_FS /tmp/arduino_build_159496/sketch/UHS_FS_NEW_DEMO.ino.cpp -o /tmp/arduino_build_159496/preproc/ctags_target_for_gcc_minus_e.cpp
/home/wwatson/arduino-1.8.15/tools-builder/ctags/5.8-arduino11/ctags -u --language-force=c++ -f - --c++-kinds=svpf --fields=KSTtzns --line-directives /tmp/arduino_build_159496/preproc/ctags_target_for_gcc_minus_e.cpp
Compiling sketch...
/home/wwatson/arduino-1.8.15/hardware/teensy/../tools/precompile_helper /home/wwatson/arduino-1.8.15/hardware/teensy/avr/cores/teensy3 /tmp/arduino_build_159496 /home/wwatson/arduino-1.8.15/hardware/teensy/../tools/arm/bin/arm-none-eabi-g++ -x c++-header -O2 -g -Wall -ffunction-sections -fdata-sections -nostdlib -mno-unaligned-access -MMD -fno-exceptions -fpermissive -felide-constructors -std=gnu++14 -Wno-error=narrowing -fno-rtti -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 -fsingle-precision-constant -D__MK66FX1M0__ -DTEENSYDUINO=154 -DARDUINO=10815 -DARDUINO_TEENSY36 -DF_CPU=180000000 -DUSB_SERIAL -DLAYOUT_US_ENGLISH -I/home/wwatson/arduino-1.8.15/hardware/teensy/avr/cores/teensy3 /tmp/arduino_build_159496/pch/Arduino.h -o /tmp/arduino_build_159496/pch/Arduino.h.gch
/home/wwatson/arduino-1.8.15/hardware/teensy/../tools/arm/bin/arm-none-eabi-g++ -c -O2 -g -Wall -ffunction-sections -fdata-sections -nostdlib -mno-unaligned-access -MMD -fno-exceptions -fpermissive -felide-constructors -std=gnu++14 -Wno-error=narrowing -fno-rtti -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 -fsingle-precision-constant -D__MK66FX1M0__ -DTEENSYDUINO=154 -DARDUINO=10815 -DARDUINO_TEENSY36 -DF_CPU=180000000 -DUSB_SERIAL -DLAYOUT_US_ENGLISH -I/tmp/arduino_build_159496/pch -I/home/wwatson/arduino-1.8.15/hardware/teensy/avr/cores/teensy3 -I/home/wwatson/Arduino/libraries/RTClib -I/home/wwatson/Arduino/libraries/UHS_host -I/home/wwatson/Arduino/libraries/ISR_safe_memory -I/home/wwatson/arduino-1.8.15/hardware/teensy/avr/libraries/Wire -I/home/wwatson/arduino-1.8.15/hardware/teensy/avr/libraries/SPI -I/home/wwatson/Arduino/libraries/UHS_ByteBuffer -I/home/wwatson/Arduino/libraries/UHS_FS /tmp/arduino_build_159496/sketch/UHS_FS_NEW_DEMO.ino.cpp -o /tmp/arduino_build_159496/sketch/UHS_FS_NEW_DEMO.ino.cpp.o
In file included from /home/wwatson/Arduino/libraries/UHS_host/UHS_host.h:64:0,
                 from /home/wwatson/Arduino/libraries/UHS_FS/examples/UHS_KINETIS_FS_HOST/UHS_FS_NEW_DEMO/UHS_FS_NEW_DEMO.ino:107:
/home/wwatson/Arduino/libraries/UHS_host/UHS_KINETIS_FS_HOST/UHS_KINETIS_FS_HOST.h:215:22: error: variable or field 'uint8_t' declared void
         virtual void uint8_t vbusPower(uint8_t port, VBUS_t state) {
                      ^
/home/wwatson/Arduino/libraries/UHS_host/UHS_KINETIS_FS_HOST/UHS_KINETIS_FS_HOST.h:215:22: error: expected ';' at end of member declaration
/home/wwatson/Arduino/libraries/UHS_host/UHS_KINETIS_FS_HOST/UHS_KINETIS_FS_HOST.h:215:66: warning: ISO C++ forbids declaration of 'vbusPower' with no type [-fpermissive]
         virtual void uint8_t vbusPower(uint8_t port, VBUS_t state) {
                                                                  ^
/home/wwatson/Arduino/libraries/UHS_host/UHS_KINETIS_FS_HOST/UHS_KINETIS_FS_HOST.h:215:30: error: conflicting return type specified for 'virtual int UHS_KINETIS_FS_HOST::vbusPower(uint8_t, VBUS_t)'
         virtual void uint8_t vbusPower(uint8_t port, VBUS_t state) {
                              ^
In file included from /home/wwatson/Arduino/libraries/UHS_host/UHS_host.h:49:0,
                 from /home/wwatson/Arduino/libraries/UHS_FS/examples/UHS_KINETIS_FS_HOST/UHS_FS_NEW_DEMO/UHS_FS_NEW_DEMO.ino:107:
/home/wwatson/Arduino/libraries/UHS_host/UHS_usbhost.h:110:32: error:   overriding 'virtual uint8_t UHS_USB_HOST_BASE::vbusPower(uint8_t, VBUS_t)'
         virtual uint8_t UHS_NI vbusPower(NOTUSED(uint8_t port), NOTUSED(VBUS_t state)) {
                                ^
Using library RTClib in folder: /home/wwatson/Arduino/libraries/RTClib (legacy)
Using library UHS_host in folder: /home/wwatson/Arduino/libraries/UHS_host (legacy)
Using library ISR_safe_memory in folder: /home/wwatson/Arduino/libraries/ISR_safe_memory (legacy)
Using library Wire at version 1.0 in folder: /home/wwatson/arduino-1.8.15/hardware/teensy/avr/libraries/Wire 
Using library SPI at version 1.0 in folder: /home/wwatson/arduino-1.8.15/hardware/teensy/avr/libraries/SPI 
Using library UHS_ByteBuffer in folder: /home/wwatson/Arduino/libraries/UHS_ByteBuffer (legacy)
Using library UHS_FS in folder: /home/wwatson/Arduino/libraries/UHS_FS (legacy)
Error compiling for board Teensy 3.6.

Maybe it's me but before the updates it would compile but when uploaded the T3.6 would hang.
 
Kinetis is for Teensy 3.[012] :) The other is for MAX3421E on SPI, and yes, you can use that (up to two MAX3421E) plus native USB at the same time, AND SDcard after 2 simple soldering modifications.
Configuring returned 00 <-- means it was successful.
mount isn't happening because your storage has a volume label. Remove the label (make it blank) and it will work with the demo.
 
@xxxajk - Cool, that did it:) With HUB and without HUB.

Output:
Code:
USB HOST READY.
No media. Waiting to mount /
/ mounted.
Removing '/HeLlO.tXt' file... completed with 4

Starting Write test...
File opened OK, fd = 1
Wrote 19 bytes, File closed result = 0.

Starting Read test...
File opened OK, fd = 1, displaying contents...
]-[ello \/\/orld!

Read completed, last read result = -1 (20), file close result = 0.
Testing rename
file rename result = 0.

Removing '/10MB.bin' file... completed with 0
10MB write timing test  10240 writes, (0), (0),  18983 ms (19 sec)
completed with 0
10MB read timing test 10240 reads, (20),  10490 ms (10 sec)
completed with 0
Directory of '/'
-rw--a     10485760 2021-08-27 16:10:14 10MB.bin
-rw--a           19 2021-08-27 16:09:54 newtest.txt
30916788224 bytes available on disk.

Flushing caches...
Remove and insert media...

Edit: Will try more devices this weekend:)
 
I'm trying to get an M.2 SSD drive working. It's a 512 GB drive, with a single 64G FAT32 partition.

During initialization, it's getting stuck in the following sequence: WaitMediaReady() -> msTestReady() -> msGetSCW().

It gets hung up waiting for the queue_Data_Transfer() to complete. It doesn't crash, but is forever waiting for the msInCompleted to be set. Any ideas of what I can further do to get this drive working?
 
I'm trying to get an M.2 SSD drive working. It's a 512 GB drive, with a single 64G FAT32 partition.
...

How is it powered? May need powered hub if not in use? Not sure how much current they eat.

Seems @mjs513 ordered an m.2 SSD some time back to test?
 
Yup... That does the trick.

Usually does. Many people don't realize that you are trying to supply power to two things plugged into one USB port, and it may have a 500mA limit of current.
After traveling down a long usb cable, power kind of ends up as much less, and then, you add another at the end of that.
Now you have effectively tried to power something that is two times the distance than it was designed for.
Sometimes even high quality cables don't help.
Normally I use powered hubs. If I have no other choice, and it has an extremely short cable on it, and I'm not going to draw more than 400mA collectively then it's usually OK.
Remember the hub eats some of the power budget, so, if you use a long cable to an unpowered hub to another long cable to the teensy, then another long cable, to yet possibly another hub... well, you can see where I'm going with this...
:cool:
 
For clarification, I have a powered hub connected to the host port, and the SSD connected to the hub and that works reliably.

The externally powering the Teensy with 5v adapter, and plugging the SSD directly into the host port also worked, but there were a couple of instances where the SSD shutdown, so not as reliable. I'll need to do more tests.
 
For clarification, I have a powered hub connected to the host port, and the SSD connected to the hub and that works reliably.

The externally powering the Teensy with 5v adapter, and plugging the SSD directly into the host port also worked, but there were a couple of instances where the SSD shutdown, so not as reliable. I'll need to do more tests.

Depending on the SSD, currently the maximum consumption (during a write) is ~17watts, or about 3 amps.
These spikes aren't sustained long, though, so you possibly could get away with a 2amp power supply and some helper capacitors on the USB power supply line.

Check the specs of the device, always add overhead.
More amps on the supply side is better.
For example, if you calculate that 1.5 amps will work, use a 2 amp supply.
 
So I've swapped in a TPD3S044DBVR, and it's still no go. The only way I can get that drive up and running is to connect it to a powered USB hub plugged into the USB port. No big deal. But this brings to light a rather serious issue that probably needs to be dealt with.

If a drive is underpowered (not enough voltage / current available), the mass storage driver will endlessly wait for the drive to come online, effectively crashing the hardware. I was able to add some code to make msGetCSW() timeout and return an error:

Code:
uint8_t USBDrive::msGetCSW(void) {
#ifdef DBGprint
	println("msGetCSW()");
#endif

	DBGPrintf("msGetCSW()"); DBGFlush();

	msCommandStatusWrapper_t StatusBlockWrapper = (msCommandStatusWrapper_t)
	{
		.Signature = CSW_SIGNATURE,
		.Tag = 0,
		.DataResidue = 0, // TODO: Proccess this if received.
		.Status = 0
	};


	queue_Data_Transfer(datapipeIn, &StatusBlockWrapper, sizeof(StatusBlockWrapper), this);
	

	int startTime = millis();

	while (!msInCompleted) {
		DBGPrintf("."); DBGFlush();
		yield();
                
                // not sure what an appropriate value here is.
		if (millis() - startTime > 2000) {
			DBGPrintf("TimeOut\n"); DBGFlush();
			return msProcessError(MS_TIMEOUT); // MS_TIMEOUT defined as 3
		}
	}

	DBGPrintf("Complete\n"); DBGFlush();

	msInCompleted = false;
	mscTransferComplete = true;
	if(StatusBlockWrapper.Signature != CSW_SIGNATURE) return msProcessError(MS_CSW_SIG_ERROR); // Signature error
	if(StatusBlockWrapper.Tag != CBWTag) return msProcessError(MS_CSW_TAG_ERROR); // Tag mismatch error
	return StatusBlockWrapper.Status;
}
 
Last edited:
So I've swapped in a TPD3S044DBVR, and it's still no go. The only way I can get that drive up and running is to connect it to a powered USB hub plugged into the USB port. No big deal. But this brings to light a rather serious issue that probably needs to be dealt with.

If a drive is underpowered (not enough voltage / current available), the mass storage driver will endlessly wait for the drive to come online, effectively crashing the hardware. I was able to add some code to make msGetCSW() timeout and return an error:

Code:
uint8_t USBDrive::msGetCSW(void) {
#ifdef DBGprint
	println("msGetCSW()");
#endif

	DBGPrintf("msGetCSW()"); DBGFlush();

	msCommandStatusWrapper_t StatusBlockWrapper = (msCommandStatusWrapper_t)
	{
		.Signature = CSW_SIGNATURE,
		.Tag = 0,
		.DataResidue = 0, // TODO: Proccess this if received.
		.Status = 0
	};


	queue_Data_Transfer(datapipeIn, &StatusBlockWrapper, sizeof(StatusBlockWrapper), this);
	

	int startTime = millis();

	while (!msInCompleted) {
		DBGPrintf("."); DBGFlush();
		yield();
                
                // not sure what an appropriate value here is.
		if (millis() - startTime > 2000) {
			DBGPrintf("TimeOut\n"); DBGFlush();
			return msProcessError(MS_TIMEOUT); // MS_TIMEOUT defined as 3
		}
	}

	DBGPrintf("Complete\n"); DBGFlush();

	msInCompleted = false;
	mscTransferComplete = true;
	if(StatusBlockWrapper.Signature != CSW_SIGNATURE) return msProcessError(MS_CSW_SIG_ERROR); // Signature error
	if(StatusBlockWrapper.Tag != CBWTag) return msProcessError(MS_CSW_TAG_ERROR); // Tag mismatch error
	return StatusBlockWrapper.Status;
}

That is a actually a good idea but there are times when different types of devices (particularly older thumb drives) can a few seconds to do internal housekeeping. Usually flushing the internal buffer. So I as well am not sure what the timeout value should be. 1 minute maybe 2. But you are right there should be a time limit. I'll add it in here and test it.
Thanks for debugging this issue.
 
Also worth mentioning that after the USBFilesystem fails during begin, I need to call the end() method; otherwise its boolean value wouldn't return to false. Seems like it should automatically be false it it fails to begin.
 
I've been working through a few more lock ups. I really think every single time you send or receive data to the flash drive there needs to be some kind of timeout implemented.

I'm implemented this function:
Code:
bool USBDrive::doWait(volatile bool& flag, int timeout_ms) {
	uint32_t start = millis();

	while (!flag) {
		yield();
		if (millis() - start > timeout_ms) return false;
	}
	return true;
}

And then wheneven the library needs to do some kind of transfer:
Code:
if (!doWait(msControlCompleted, 5000)) msProcessError(3); // or some other error code
 
Last edited:
I found another issue in claim.

Code:
        datapipeIn = new_Pipe(dev, 2, endpointIn, 1, packetSizeIn, intervalIn);
	datapipeOut = new_Pipe(dev, 2, endpointOut, 0, packetSizeOut, intervalOut);

        //need to check these!
	if (!datapipeIn || !datapipeOut) {
		println("There's no pipe"); Serial.flush();
		return false;
	}
 
I now have things very stable. 15 low voltage power cycles in a row without a lockup. The last issue I found that was causing a lock-up is related to compiling with link time optimization. I haven't been able to track down where exactly the problem is though.
 
I've got it rock solid now... Very snappy, and no lockups even with LTO. I had to make quite a few changes. When I have more time, I'll zip the library up and send it to you.
 
Do what you want with this. I don't mind if you don't want to use it. Here's the updated section of USBHost_t36.
Code:
class USBDrive : public USBDriver, public FsBlockDeviceInterface {
public:
	USBDrive(USBHost &host) { init(); }
	USBDrive(USBHost *host) { init(); }


	msSCSICapacity_t msCapacity;
	msInquiryResponse_t msInquiry;
	msRequestSenseResponse_t msSense;
	msDriveInfo_t msDriveInfo;

	bool mscTransferComplete = false;
	uint8_t mscInit(void);

	//***Changed this to bool so it could return success or fail
	bool msReset(void);

	//***Changed this to bool so it could return success or fail
	bool msGetMaxLun(uint8_t& maxLun);

	void msCurrentLun(uint8_t lun) {currentLUN = lun;}
	uint8_t msCurrentLun() {return currentLUN;}
	bool available() { delay(0); return deviceAvailable; }
	uint8_t checkConnectedInitialized(void);
	uint16_t getIDVendor() {return idVendor; }
	uint16_t getIDProduct() {return idProduct; }
	uint8_t getHubNumber() { return hubNumber; }
	uint8_t getHubPort() { return hubPort; }
	uint8_t getDeviceAddress() { return deviceAddress; }
	uint8_t WaitMediaReady();
	uint8_t msTestReady();
	uint8_t msReportLUNs(uint8_t *Buffer);
	uint8_t msStartStopUnit(uint8_t mode);
	uint8_t msReadDeviceCapacity(msSCSICapacity_t * const Capacity);
	uint8_t msDeviceInquiry(msInquiryResponse_t * const Inquiry);
	uint8_t msProcessError(uint8_t msStatus);
	uint8_t msRequestSense(msRequestSenseResponse_t * const Sense);
	uint8_t msRequestSense(void *Sense);

	uint8_t msReadBlocks(const uint32_t BlockAddress, const uint16_t Blocks,
						 const uint16_t BlockSize, void * sectorBuffer);
	uint8_t msReadSectorsWithCB(const uint32_t BlockAddress, const uint16_t Blocks, void (*callback)(uint32_t token, uint8_t* data), uint32_t token);
	uint8_t msWriteBlocks(const uint32_t BlockAddress, const uint16_t Blocks,
                        const uint16_t BlockSize,	const void * sectorBuffer);

	bool begin();
	// Not sure of good name here.  
	// maybe startFilesystems(), enumFileSystems()...
	bool startFilesystems();
	bool filesystemsStarted() {return _filesystems_started;}

	void printPartionTable(Print &Serialx);
	void printExtendedPartition(MbrSector_t *mbr, uint8_t ipExt, Print &Serialx);
	uint32_t printGUIDPartitionTable(Print &Serialx);

	enum {INVALID_VOL=0, MBR_VOL, EXT_VOL, GPT_VOL}; // what type of volume did the mapping return
	int findPartition(int partition, int &type, uint32_t &firstSector, uint32_t &numSectors, 
				   uint32_t &mbrLBA, uint8_t &mbrPart, uint8_t *guid=nullptr);

	static void filesystem_ready_for_drive(USBFSBase *fsbase);
	void filesystem_assign_to_drive(USBFSBase *fsbase, bool busy);

public:
	// Functions for SdFat FsBlockDeviceInterface
	// return the number of 512 byte sectors for the whole drive
	uint32_t sectorCount() { return msDriveInfo.capacity.Blocks; }
	// return code for the last error.  (where is list of errors?)
	uint8_t errorCode() const { return m_errorCode; }
	// return error data for last error.
	uint32_t errorData() const { return 0; }
	// return error line for last error. Tmp function for debug.
	uint32_t errorLine() const { return m_errorLine; }
	// Check for busy
	bool isBusy() { return !m_initDone && !mscTransferComplete; }
	// Check for busy with MSC read operation
	bool isBusyRead() { return mscTransferComplete; }
	// Check for busy with MSC read operation
	bool isBusyWrite() { return mscTransferComplete; }
	// Read a USB drive's information. This contains the drive's identification
	// information such as Manufacturer ID, Product name, Product serial
	// number and Manufacturing date pluse more.
	bool readUSBDriveInfo(msDriveInfo_t * driveInfo) {
		memcpy(driveInfo, &msDriveInfo, sizeof(msDriveInfo_t));
		return true;
	}
	// Read a 512 byte sector from an USB MSC drive.
	bool readSector(uint32_t sector, uint8_t* dst);
	// Read multiple 512 byte sectors from an USB MSC drive.
	bool readSectors(uint32_t sector, uint8_t* dst, size_t numsectors);
	// return USB MSC drive status.
	uint32_t status() { return m_errorCode; }
	// return success if sync successful. Not for user apps.
	bool syncDevice() { return true; }
	// Writes a 512 byte sector to an USB MSC drive.
	bool writeSector(uint32_t sector, const uint8_t* src);
	// Write multiple 512 byte sectors to an USB MSC drive.
	bool writeSectors(uint32_t sector, const uint8_t* src, size_t ns);
	// Read multiple 512 byte sectors from an USB MSC drive, using
	// a callback per sector
	bool readSectorsWithCB(uint32_t sector, size_t ns,
		void (*callback)(uint32_t, uint8_t *), uint32_t token);
	bool readSectorsCallback(uint32_t sector, uint8_t* dst, size_t numSectors,
		void (*callback)(uint32_t sector, uint8_t *buf, void *context), void *context);
	//bool writeSectorsCallback(uint32_t sector, size_t numSectors,
	//	const uint8_t * (*callback)(uint32_t sector, void *context), void *context);

protected:

	//** THESE ARE THE FUNCTIONS I'VE ADDED
	bool handleDataInTransfer(msCommandStatusWrapper_t* block, uint32_t size, uint32_t timeout, const char* sender);
	bool handleDataOutTransfer(msCommandBlockWrapper_t* block, uint32_t size, uint32_t timeout, const char* sender);
	bool handleControlTransfer(Device_t* device, setup_t* setup, void* buf, uint32_t timeout, const char* sender);
	bool handleWait(volatile bool& flag, int timeout_ms, const char* sender);

	virtual bool claim(Device_t *device, int type, const uint8_t *descriptors, uint32_t len);
	virtual void control(const Transfer_t *transfer);
	virtual void disconnect();
	static void callbackIn(const Transfer_t *transfer);
	static void callbackOut(const Transfer_t *transfer);
	void new_dataIn(const Transfer_t *transfer);
	void new_dataOut(const Transfer_t *transfer);
	void init();
	uint8_t msDoCommand(msCommandBlockWrapper_t *CBW, void *buffer);
	uint8_t msGetCSW(void);
private:
	Pipe_t mypipes[3] __attribute__ ((aligned(32)));
	Transfer_t mytransfers[7] __attribute__ ((aligned(32)));
	strbuf_t mystring_bufs[1];
	uint32_t packetSizeIn;
	uint32_t packetSizeOut;
	Pipe_t *datapipeIn;
	Pipe_t *datapipeOut;
	uint8_t bInterfaceNumber;
	uint32_t endpointIn = 0;
	uint32_t endpointOut = 0;
	setup_t setup;
	uint8_t report[8];
	uint8_t maxLUN = 0;
	uint8_t currentLUN = 0;
//	msSCSICapacity_t msCapacity;
//	msInquiryResponse_t msInquiry;
//	msRequestSenseResponse_t msSense;
	uint16_t idVendor = 0;
	uint16_t idProduct = 0;
	uint8_t hubNumber = 0;
	uint8_t hubPort = 0;
	uint8_t deviceAddress = 0;
	volatile bool msOutCompleted = false;
	volatile bool msInCompleted = false;
	volatile bool msControlCompleted = false;
	uint32_t CBWTag = 0;
	bool deviceAvailable = false;
	// experiment with transfers with callbacks.
	void (*_read_sectors_callback)(uint32_t token, uint8_t* data) = nullptr;
	uint32_t _read_sectors_token = 0;
	uint16_t _read_sectors_remaining = 0;
	enum {READ_CALLBACK_TIMEOUT_MS=250};
	elapsedMillis _emlastRead;
	uint8_t _read_sector_buffer1[512];
	uint8_t _read_sector_buffer2[512];
	bool m_initDone = false;
	uint8_t m_errorCode = MS_NO_MEDIA_ERR;
	uint32_t m_errorLine = 0;
	USBFSBase *claimed_filesystem_list = nullptr;
	bool _filesystems_started = false;
	int _cGPTParts = 0;  // if GPT cache of parts.

	static USBFSBase *available_filesystem_list;

};

Here is the MassStorageDriver.cpp
Code:
/* MSC Teensy36 USB Host Mass Storage library
 * Copyright (c) 2017-2019 Warren Watson.
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
 * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
 * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

//MassStorageDriver.cpp

// HOW LONG TO WAIT UNTIL TIMEOUT DURING TRNASFERS
#define TRANSFER_WAIT_TIME 5000

#include <Arduino.h>
#include "USBHost_t36.h"  // Read this header first for key info
#include "utility/USBFilesystemFormatter.h"
#define print   USBHost::print_
#define println USBHost::println_

// I added this define to that I could use this library without Serial being available
#define MASS_STORAGE_PRINT_DEBUG

// Uncomment this to display function usage and sequencing.
#define DBGprint 1
#ifdef DBGprint
#define DBGPrintf Serial.printf
#define DBGFlush() Serial.flush()
#else
void inline DBGPrintf(...) {};
void inline DBGFlush() {};
#endif



USBFSBase *USBDrive::available_filesystem_list = nullptr;

static const uint8_t mbdpGuid[16] PROGMEM = {0xA2, 0xA0, 0xD0, 0xEB, 0xE5, 0xB9, 0x33, 0x44, 0x87, 0xC0, 0x68, 0xB6, 0xB7, 0x26, 0x99, 0xC7};

// Big Endian/Little Endian
#define swap32(x) ((x >> 24) & 0xff) | \
				  ((x << 8) & 0xff0000) | \
				  ((x >> 8) & 0xff00) |  \
                  ((x << 24) & 0xff000000)


//*** THESE ARE THE METHODS I ADDED
bool USBDrive::handleWait(volatile bool& flag, int timeout_ms, const char* sender) {


	DBGPrintf(sender); DBGFlush();

	uint32_t start = millis();
	uint32_t lastMark = start;

	while (!flag) {

		yield();

		if (millis() - lastMark > 100) {
			lastMark = millis();
			DBGPrintf("."); DBGFlush();
		}

		if (millis() - start > timeout_ms) {
			DBGPrintf("timeout!\n"); DBGFlush();
			return false;
		}
	}

	DBGPrintf("completed\n"); DBGFlush();

	return true;
}

bool USBDrive::handleControlTransfer(Device_t* device, setup_t* setup, void* buf, uint32_t timeout, const char* sender) {

	int attempts = 0;
	while (1) {

		attempts++;
		if (queue_Control_Transfer(device, setup, buf, this)) {
			return handleWait(msControlCompleted, timeout, sender);
		}

		delay(10);
		if (attempts > 10)
			return false;
		else {
			DBGPrintf("...retrying\n"); DBGFlush();
		}
	}

	DBGPrintf("Couldn't transfer\n"); DBGFlush();

	return false;
}

bool USBDrive::handleDataInTransfer(msCommandStatusWrapper_t* block, uint32_t size, uint32_t timeout, const char* sender) {

	int attempts = 0;
	while (1) {

		attempts++;
		if (queue_Data_Transfer(datapipeIn, block, size, this)) {

			return handleWait(msInCompleted, timeout, sender);
		}

		delay(10);
		if (attempts > 10)
			return false;
		else {

			DBGPrintf("...retrying\n"); DBGFlush();
		}
	}

	DBGPrintf("Couldn't transfer\n"); DBGFlush();

	return false;
}

bool USBDrive::handleDataOutTransfer(msCommandBlockWrapper_t* block, uint32_t size, uint32_t timeout, const char* sender) {

	int attempts = 0;
	while (1) {

		attempts++;
		if (queue_Data_Transfer(datapipeOut, block, size, this)) {

			return handleWait(msOutCompleted, timeout, sender);
		}

		delay(10);
		if (attempts > 10)
			return false;
		else {

			DBGPrintf("...retrying\n"); DBGFlush();
		}
	}

	DBGPrintf("Couldn't transfer\n"); DBGFlush();

	return false;
}



void USBDrive::init()
{
	contribute_Pipes(mypipes, sizeof(mypipes)/sizeof(Pipe_t));
	contribute_Transfers(mytransfers, sizeof(mytransfers)/sizeof(Transfer_t));
	contribute_String_Buffers(mystring_bufs, sizeof(mystring_bufs)/sizeof(strbuf_t));
	driver_ready_for_device(this);
}



void USBDrive::filesystem_ready_for_drive(USBFSBase *fsbase)
{
	fsbase->next = NULL;
	if (available_filesystem_list == NULL) {
		available_filesystem_list = fsbase;
	} else {
		USBFSBase *last = available_filesystem_list;
		while (last->next) last = last->next;
		last->next = fsbase;
	}	
}

void USBDrive::filesystem_assign_to_drive(USBFSBase *fsbase, bool assign)
{
	USBFSBase *prev = nullptr;
	if (assign) {
		// We need to remove rom the free list
		USBFSBase *fs = available_filesystem_list;

		while (fs && (fs != fsbase)) {
			prev = fs;
			fs = fs->next;
		}
		if (fs) {
			// unlink it
			if (prev) prev->next = fs->next;
			else available_filesystem_list = fs->next;
			// add it to the Drives claimed list
			fs->next = claimed_filesystem_list;
			claimed_filesystem_list = fs;
		}
	} else {
		// lets do the reverse and try to remove from drive list.
		USBFSBase *fs = claimed_filesystem_list;
		while (fs && (fs != fsbase)) {
			prev = fs;
			fs = fs->next;
		}
		if (fs) {
			// unlink it
			if (prev) prev->next = fs->next;
			else claimed_filesystem_list = fs->next;
			// add it to the Drives claimed list
			fs->next = available_filesystem_list;
			available_filesystem_list = fs;
		}
	}
}


bool USBDrive::claim(Device_t *dev, int type, const uint8_t *descriptors, uint32_t len)
{
	println("USBDrive claim this=", (uint32_t)this, HEX);
	// only claim at interface level

	if (type != 1) return false;
	if (len < 9+7+7) return false; // Interface descriptor + 2 endpoint decriptors 

	print_hexbytes(descriptors, len);

	uint32_t numendpoint = descriptors[4];
	if (numendpoint < 1) return false; 
	if (descriptors[5] != 8) return false; // bInterfaceClass, 8 = MASS Storage class
	if (descriptors[6] != 6) return false; // bInterfaceSubClass, 6 = SCSI transparent command set (SCSI Standards)
	if (descriptors[7] != 80) return false; // bInterfaceProtocol, 80 = BULK-ONLY TRANSPORT

	bInterfaceNumber = descriptors[2];

	uint8_t desc_index = 9;
	uint8_t in_index = 0xff, out_index = 0xff;

	println("numendpoint=", numendpoint, HEX);
	while (numendpoint--) {
		if ((descriptors[desc_index] != 7) || (descriptors[desc_index+1] != 5)) return false; // not an end point
		if (descriptors[desc_index+3] == 2) {  // Bulk end point
			if (descriptors[desc_index+2] & 0x80)
				in_index = desc_index;
			else
				out_index = desc_index;
		}
		desc_index += 7;	// point to next one...
	}
	if ((in_index == 0xff) || (out_index == 0xff)) return false;	// did not find end point
	endpointIn = descriptors[in_index+2]; // bulk-in descriptor 1 81h
	endpointOut = descriptors[out_index+2]; // bulk-out descriptor 2 02h

	println("endpointIn=", endpointIn, HEX);
	println("endpointOut=", endpointOut, HEX);

	uint32_t sizeIn = descriptors[in_index+4] | (descriptors[in_index+5] << 8);
	println("packet size in (USBDrive) = ", sizeIn);

	uint32_t sizeOut = descriptors[out_index+4] | (descriptors[out_index+5] << 8);
	println("packet size out (USBDrive) = ", sizeOut);
	packetSizeIn = sizeIn;	
	packetSizeOut = sizeOut;	

	uint32_t intervalIn = descriptors[in_index+6];
	uint32_t intervalOut = descriptors[out_index+6];

	println("polling intervalIn = ", intervalIn);
	println("polling intervalOut = ", intervalOut);
	datapipeIn = new_Pipe(dev, 2, endpointIn, 1, packetSizeIn, intervalIn);

	if (!datapipeIn) {
		DBGPrintf("USBDrive: Couldn't create datapipeIn.");
		return false;
	}
	datapipeOut = new_Pipe(dev, 2, endpointOut, 0, packetSizeOut, intervalOut);

	if (!datapipeOut) {
		DBGPrintf("USBDrive: Couldn't create datapipeOut.");
		return false; // Possible memory leak?  Who is responsible for deleting patapipeIn?
	}

	datapipeIn->callback_function = callbackIn;
	datapipeOut->callback_function = callbackOut;

	idVendor = dev->idVendor;
	idProduct = dev->idProduct;
	hubNumber = dev->hub_address;
	deviceAddress = dev->address;
	hubPort = dev->hub_port; // Used for device ID with multiple drives.

	msOutCompleted = false;
	msInCompleted = false;
	msControlCompleted = false;
	deviceAvailable = true;
	msDriveInfo.initialized = false;
	msDriveInfo.connected = true;
	_cGPTParts = 0; // have not cached this yet GPT
#ifdef DBGprint
	print("   connected = ");
	println(msDriveInfo.connected);
	print("   initialized = ");
	println(msDriveInfo.initialized);
#endif	

	println("Finished Mass Storage claim.");
	
	return true;
}

void USBDrive::disconnect()
{
	// We need to go through and release an patitions we are holding onto.
	USBFSBase *usbfs = claimed_filesystem_list; 
	while (usbfs) {
		usbfs->releasePartition(); // lets release the partition.
		USBFSBase *next = usbfs->next;

		usbfs->mydevice = nullptr;
		usbfs->next = available_filesystem_list;
		available_filesystem_list = usbfs;
		usbfs = next;
	}
	claimed_filesystem_list = nullptr;
 	_filesystems_started = false;

	deviceAvailable = false;
	println("Device Disconnected...");
	msDriveInfo.connected = false;
	msDriveInfo.initialized = false;
	memset(&msDriveInfo, 0, sizeof(msDriveInfo_t));

#ifdef DBGprint
	print("   connected ");
	println(msDriveInfo.connected);
	print("   initialized ");
	println(msDriveInfo.initialized);
#endif
}

void USBDrive::control(const Transfer_t *transfer)
{
	println("control CallbackIn (USBDrive)");
	print_hexbytes(report, 8);
	msControlCompleted = true;

}

void USBDrive::callbackIn(const Transfer_t *transfer)
{
	println("USBDrive CallbackIn (static)");
	if (transfer->driver) {
		print("transfer->qtd.token = ");
		println(transfer->qtd.token & 255);
		((USBDrive *)(transfer->driver))->new_dataIn(transfer);
	}
}

void USBDrive::callbackOut(const Transfer_t *transfer)
{
	println("USBDrive CallbackOut (static)");
	if (transfer->driver) {
		print("transfer->qtd.token = ");
		println(transfer->qtd.token & 255);
		((USBDrive *)(transfer->driver))->new_dataOut(transfer);
	}
}

void USBDrive::new_dataOut(const Transfer_t *transfer)
{
	uint32_t len = transfer->length - ((transfer->qtd.token >> 16) & 0x7FFF);
	println("USBDrive dataOut (static)", len, DEC);
	print_hexbytes((uint8_t*)transfer->buffer, (len < 32)? len : 32 );
	msOutCompleted = true; // Last out transaction is completed.
}

void USBDrive::new_dataIn(const Transfer_t *transfer)
{
	uint32_t len = transfer->length - ((transfer->qtd.token >> 16) & 0x7FFF);
	println("USBDrive dataIn (static): ", len, DEC);
	print_hexbytes((uint8_t*)transfer->buffer, (len < 32)? len : 32 );
	if (_read_sectors_callback) {
		_emlastRead = 0; // remember that we received something. 
		(*_read_sectors_callback)(_read_sectors_token, (uint8_t*)transfer->buffer);
		_read_sectors_remaining--;
		if (_read_sectors_remaining > 1) queue_Data_Transfer(datapipeIn, transfer->buffer, len, this);
		if (!_read_sectors_remaining) {
			_read_sectors_callback = nullptr;
			msInCompleted = true; // Last in transaction is completed.
		}
#if defined(DBGprint) && (DBGprint > 1)
		Serial.write('@');
		if ((_read_sectors_remaining & 0x3f) == 0) Serial.printf("\n");
#endif
	}
	else msInCompleted = true; // Last in transaction is completed.
}

// Initialize Mass Storage Device
uint8_t USBDrive::mscInit(void) {
#ifdef DBGprint
	println("mscIint()");
	DBGFlush();
#endif
	uint8_t msResult = MS_CBW_PASS;

	CBWTag = 0;
	uint32_t start = millis();
	// Check if device is connected.
	do {
		if ((millis() - start) >= MSC_CONNECT_TIMEOUT) {
			return MS_NO_MEDIA_ERR;  // Not connected Error.
		}
		yield();
	} while(!available());
  
	if (!msReset()) return msProcessError(MS_CBW_TIMEOUT);

	delay(100);
	if (!msGetMaxLun(maxLUN)) return msProcessError(MS_CBW_TIMEOUT);

	delay(100);

	//-------------------------------------------------------
	if (msStartStopUnit(1) == MS_CBW_TIMEOUT) return MS_CBW_TIMEOUT;

	msResult = WaitMediaReady();

	if (msResult)
		return msResult;
		
	// Retrieve drive information.
	msDriveInfo.initialized = true;
	msDriveInfo.hubNumber = getHubNumber();			// Which HUB.
	msDriveInfo.hubPort = getHubPort();				// Which HUB port.
	msDriveInfo.deviceAddress = getDeviceAddress();	// Device addreess.
	msDriveInfo.idVendor = getIDVendor();  			// USB Vendor ID.
	msDriveInfo.idProduct = getIDProduct();  		// USB Product ID.
	msResult = msDeviceInquiry(&msInquiry);			// Config Info.
	if(msResult)
		return msResult;
	msResult = msReadDeviceCapacity(&msCapacity);	// Size Info.
	if(msResult)
		return msResult;
	memcpy(&msDriveInfo.inquiry, &msInquiry, sizeof(msInquiryResponse_t));
	memcpy(&msDriveInfo.capacity, &msCapacity, sizeof(msSCSICapacity_t));



	return msResult;
}



//---------------------------------------------------------------------------
// Perform Mass Storage Reset
bool USBDrive::msReset(void) {
#ifdef DBGprint
	println("msReset()");
#endif
	DBGPrintf(">>msReset()\n"); DBGFlush();
	mk_setup(setup, 0x21, 0xff, 0, bInterfaceNumber, 0);

	if (!handleControlTransfer(device, &setup, NULL, TRANSFER_WAIT_TIME, "msReset()")) return false;

	msControlCompleted = false;
	return true;
}

//---------------------------------------------------------------------------
// Get MAX LUN
bool USBDrive::msGetMaxLun(uint8_t& maxLUN) {

	report[0] = 0;
	mk_setup(setup, 0xa1, 0xfe, 0, bInterfaceNumber, 1);

	if (!handleControlTransfer(device, &setup, report, TRANSFER_WAIT_TIME, "msGetMaxLun()")) return false;

	msControlCompleted = false;
	maxLUN = report[0];
	return true;
}

uint8_t USBDrive::WaitMediaReady() {
	uint8_t msResult;
	uint32_t start = millis();

	do {
		if((millis() - start) >= MEDIA_READY_TIMEOUT) {
			return MS_UNIT_NOT_READY;  // Not Ready Error.
		}
		msResult = msTestReady();
		
		yield();

	 } while (msResult == 1);

	return msResult;
}

// Check if drive is connected and Initialized.
uint8_t USBDrive::checkConnectedInitialized(void) {
	uint8_t msResult = MS_CBW_PASS;
#ifdef DBGprint
	print("checkConnectedInitialized()");
#endif
	if (!msDriveInfo.connected) {
		return MS_NO_MEDIA_ERR;
	}
	
	if (!msDriveInfo.initialized) {
		msResult = mscInit();
		if (msResult != MS_CBW_PASS) return MS_UNIT_NOT_READY; // Not Initialized
	}
	return MS_CBW_PASS;
}

//---------------------------------------------------------------------------
// Send SCSI Command
// Do a complete 3 stage transfer.
uint8_t USBDrive::msDoCommand(msCommandBlockWrapper_t *CBW,	void *buffer)
{
	uint8_t CSWResult = 0;
	mscTransferComplete = false;

	if(CBWTag == 0xFFFFFFFF) CBWTag = 1;
	// digitalWriteFast(2, HIGH);
	queue_Data_Transfer(datapipeOut, CBW, sizeof(msCommandBlockWrapper_t), this); // Command stage.

	if (!handleWait(msOutCompleted, TRANSFER_WAIT_TIME,"msDoCommand()")) msProcessError(3);


	// digitalWriteFast(2, LOW);
	msOutCompleted = false;
	if ((CBW->Flags == CMD_DIR_DATA_IN)) { // Data stage from device.
		queue_Data_Transfer(datapipeIn, buffer, CBW->TransferLength, this);

		if (!handleWait(msInCompleted, TRANSFER_WAIT_TIME, "msDoCommand()")) msProcessError(3);

		// digitalWriteFast(2, HIGH);
		msInCompleted = false;
	} 
	else {							  // Data stage to device.
		queue_Data_Transfer(datapipeOut, buffer, CBW->TransferLength, this);
	
		if (!handleWait(msOutCompleted, TRANSFER_WAIT_TIME, "msDoCommand()")) msProcessError(3);

		// digitalWriteFast(2, LOW);
		msOutCompleted = false;
	}

	CSWResult = msGetCSW(); // Status stage.
	// All stages of this transfer have completed.
	//Check for special cases. 
	//If test for unit ready command is given then
	//  return the CSW status byte.
	//Bit 0 == 1 == not ready else
	//Bit 0 == 0 == ready.
	//And the Start/Stop Unit command as well.
	if((CBW->CommandData[0] == CMD_TEST_UNIT_READY) ||
	   (CBW->CommandData[0] == CMD_START_STOP_UNIT))
		return CSWResult;
	else // Process possible SCSI errors.
		return msProcessError(CSWResult);
}

//---------------------------------------------------------------------------
// Get Command Status Wrapper
uint8_t USBDrive::msGetCSW(void) {

	msCommandStatusWrapper_t StatusBlockWrapper = (msCommandStatusWrapper_t)
	{
		.Signature = CSW_SIGNATURE,
		.Tag = 0,
		.DataResidue = 0, // TODO: Proccess this if received.
		.Status = 0
	};


	if (!handleDataInTransfer(&StatusBlockWrapper, sizeof(StatusBlockWrapper), TRANSFER_WAIT_TIME, "msGetCSW()"))
		return msProcessError(3);


	msInCompleted = false;
	mscTransferComplete = true;
	if(StatusBlockWrapper.Signature != CSW_SIGNATURE) return msProcessError(MS_CSW_SIG_ERROR); // Signature error
	if(StatusBlockWrapper.Tag != CBWTag) return msProcessError(MS_CSW_TAG_ERROR); // Tag mismatch error
	return StatusBlockWrapper.Status;
}

//---------------------------------------------------------------------------
// Test Unit Ready
uint8_t USBDrive::msTestReady() {

	msCommandBlockWrapper_t CommandBlockWrapper = (msCommandBlockWrapper_t)
	{
		.Signature          = CBW_SIGNATURE,
		.Tag                = ++CBWTag,
		.TransferLength     = 0,
		.Flags              = CMD_DIR_DATA_IN,
		.LUN                = currentLUN,
		.CommandLength      = 6,
		.CommandData        = {CMD_TEST_UNIT_READY, 0x00, 0x00, 0x00, 0x00, 0x00}
	};

	if (!handleDataOutTransfer(&CommandBlockWrapper, sizeof(CommandBlockWrapper), 10000, "msTestReady()")) 
		return msProcessError(3);

	
	msOutCompleted = false;
	//return 0;
	return msGetCSW();
}

//---------------------------------------------------------------------------
// Start/Stop unit
uint8_t USBDrive::msStartStopUnit(uint8_t mode) {

	msCommandBlockWrapper_t CommandBlockWrapper = (msCommandBlockWrapper_t)
	{
		.Signature          = CBW_SIGNATURE,
		.Tag                = ++CBWTag,
		.TransferLength     = 0,
		.Flags              = CMD_DIR_DATA_IN,
		.LUN                = currentLUN,
		.CommandLength      = 6,
		.CommandData        = {CMD_START_STOP_UNIT, 0x01, 0x00, 0x00, mode, 0x00}
	};

	if (!handleDataOutTransfer(&CommandBlockWrapper, sizeof(CommandBlockWrapper), 10000, "msStartStopUnit()"))
		return msProcessError(3);

	msOutCompleted = false;
	return msGetCSW();
}

//---------------------------------------------------------------------------
// Read Mass Storage Device Capacity (Number of Blocks and Block Size)
uint8_t USBDrive::msReadDeviceCapacity(msSCSICapacity_t * const Capacity) {
#ifdef DBGprint
	println("msReadDeviceCapacity()");
#endif
	uint8_t result = 0;
	msCommandBlockWrapper_t CommandBlockWrapper = (msCommandBlockWrapper_t)
	{
		.Signature          = CBW_SIGNATURE,
		.Tag                = ++CBWTag,
		.TransferLength     = sizeof(msSCSICapacity_t),
		.Flags              = CMD_DIR_DATA_IN,
		.LUN                = currentLUN,
		.CommandLength      = 10,
		.CommandData        = {CMD_RD_CAPACITY_10,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}
	};
	result = msDoCommand(&CommandBlockWrapper, Capacity);
	Capacity->Blocks = swap32(Capacity->Blocks);
	Capacity->BlockSize = swap32(Capacity->BlockSize);
	return result;
}

//---------------------------------------------------------------------------
// Do Mass Storage Device Inquiry
uint8_t USBDrive::msDeviceInquiry(msInquiryResponse_t * const Inquiry)
{
#ifdef DBGprint
	println("msDeviceInquiry()");
#endif
	msCommandBlockWrapper_t CommandBlockWrapper = (msCommandBlockWrapper_t)
	{
		.Signature          = CBW_SIGNATURE,
		.Tag                = ++CBWTag,
		.TransferLength     = sizeof(msInquiryResponse_t),
		.Flags              = CMD_DIR_DATA_IN,
		.LUN                = currentLUN,
		.CommandLength      = 6,
		.CommandData        = {CMD_INQUIRY,0x00,0x00,0x00,sizeof(msInquiryResponse_t),0x00}
	};
	return msDoCommand(&CommandBlockWrapper, Inquiry);
}

//---------------------------------------------------------------------------
// Request Sense Data
uint8_t USBDrive::msRequestSense(msRequestSenseResponse_t * const Sense)
{
#ifdef DBGprint
	println("msRequestSense()");
#endif
	msCommandBlockWrapper_t CommandBlockWrapper = (msCommandBlockWrapper_t)
	{
		.Signature          = CBW_SIGNATURE,
		.Tag                = ++CBWTag,
		.TransferLength     = sizeof(msRequestSenseResponse_t),
		.Flags              = CMD_DIR_DATA_IN,
		.LUN                = currentLUN,
		.CommandLength      = 6,
		.CommandData        = {CMD_REQUEST_SENSE, 0x00, 0x00, 0x00, sizeof(msRequestSenseResponse_t), 0x00}
	};
	return msDoCommand(&CommandBlockWrapper, Sense);
}

//---------------------------------------------------------------------------
// Report LUNs
uint8_t USBDrive::msReportLUNs(uint8_t *Buffer)
{
#ifdef DBGprint
	println("msReportLuns()");
#endif
	msCommandBlockWrapper_t CommandBlockWrapper = (msCommandBlockWrapper_t)
	{
		.Signature          = CBW_SIGNATURE,
		.Tag                = ++CBWTag,
		.TransferLength     = MAXLUNS,
		.Flags              = CMD_DIR_DATA_IN,
		.LUN                = currentLUN,
		.CommandLength      = 12,
		.CommandData        = {CMD_REPORT_LUNS, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, MAXLUNS, 0x00, 0x00}
	};
	return msDoCommand(&CommandBlockWrapper, Buffer);
}


//---------------------------------------------------------------------------
// Read Sectors (Multi Sector Capable)
uint8_t USBDrive::msReadBlocks(
									const uint32_t BlockAddress,
									const uint16_t Blocks,
									const uint16_t BlockSize,
									void * sectorBuffer)
	{
	println("msReadBlocks()");
#if defined(DBGprint) && (DBGprint > 1)
	Serial.printf("<<< msReadBlocks(%x %x %x)\n", BlockAddress, Blocks, BlockSize);
#endif
	uint8_t BlockHi = (Blocks >> 8) & 0xFF;
	uint8_t BlockLo = Blocks & 0xFF;
	msCommandBlockWrapper_t CommandBlockWrapper = (msCommandBlockWrapper_t)
	{
		.Signature          = CBW_SIGNATURE,
		.Tag                = ++CBWTag,
		.TransferLength     = (uint32_t)(Blocks * BlockSize),
		.Flags              = CMD_DIR_DATA_IN,
		.LUN                = currentLUN,
		.CommandLength      = 10,
		.CommandData        = {CMD_RD_10, 0x00,
							  (uint8_t)(BlockAddress >> 24),
							  (uint8_t)(BlockAddress >> 16),
							  (uint8_t)(BlockAddress >> 8),
							  (uint8_t)(BlockAddress & 0xFF),
							   0x00, BlockHi, BlockLo, 0x00}
	};
	#if defined(__IMXRT1062__)
	if ((uint32_t)sectorBuffer >= 0x20200000u)  arm_dcache_flush_delete(sectorBuffer, CommandBlockWrapper.TransferLength);
	#endif
	return msDoCommand(&CommandBlockWrapper, sectorBuffer);
}

//---------------------------------------------------------------------------
// Read Sectors (Multi Sector Capable)

uint8_t USBDrive::msReadSectorsWithCB(
			const uint32_t BlockAddress,
			const uint16_t Blocks,
			void (*callback)(uint32_t, uint8_t *),
			uint32_t token)
	{
#if defined(DBGprint) && (DBGprint > 1)
	Serial.printf("<<< msReadSectorsWithCB(%x %u %x)\n", BlockAddress, Blocks, (uint32_t)callback);
#endif
	if ((callback == nullptr) || (!Blocks)) return MS_CBW_FAIL;

	uint8_t BlockHi = (Blocks >> 8) & 0xFF;
	uint8_t BlockLo = Blocks & 0xFF;
	static const uint16_t BlockSize = 512;

	msCommandBlockWrapper_t CommandBlockWrapper = (msCommandBlockWrapper_t)
	{
	
		.Signature          = CBW_SIGNATURE,
		.Tag                = ++CBWTag,
		.TransferLength     = (uint32_t)(Blocks * BlockSize),
		.Flags              = CMD_DIR_DATA_IN,
		.LUN                = currentLUN,
		.CommandLength      = 10,
		.CommandData        = {CMD_RD_10, 0x00,
					  (uint8_t)(BlockAddress >> 24),
					  (uint8_t)(BlockAddress >> 16),
					  (uint8_t)(BlockAddress >> 8),
					  (uint8_t)(BlockAddress & 0xFF),
					   0x00, BlockHi, BlockLo, 0x00}
	};

	// We need to remember how many blocks and call back function
	_read_sectors_callback = callback;
	_read_sectors_remaining = Blocks;
	_read_sectors_token = token;
	_emlastRead = 0; // reset the timeout. 

	// lets unwrap the msDoCommand here...
	uint8_t CSWResult = 0;
	mscTransferComplete = false;

	if(CBWTag == 0xFFFFFFFF) CBWTag = 1;
	// digitalWriteFast(2, HIGH);
	queue_Data_Transfer(datapipeOut, &CommandBlockWrapper, sizeof(msCommandBlockWrapper_t), this); // Command stage.

	while(!msOutCompleted && (_emlastRead < READ_CALLBACK_TIMEOUT_MS)) yield();
	// digitalWriteFast(2, LOW);

	msOutCompleted = false;

	queue_Data_Transfer(datapipeIn, _read_sector_buffer1, BlockSize, this);
	if (_read_sectors_remaining > 1) {
		queue_Data_Transfer(datapipeIn, _read_sector_buffer2, BlockSize, this);
	}

	while(!msInCompleted && (_emlastRead < READ_CALLBACK_TIMEOUT_MS)) ;
	// digitalWriteFast(2, HIGH);

	if (!msInCompleted) {
		// clear this out..
		#ifdef DBGprint
			Serial.printf("!!! msReadBlocks Timed Out(%u)\n", _read_sectors_remaining);
		#endif
		_read_sectors_callback = nullptr;
		_read_sectors_remaining = 0;
		return MS_CBW_FAIL;
	}	

	msInCompleted = false;

	CSWResult = msGetCSW(); // Status stage.
	#ifdef DBGprint
	Serial.printf("  CSWResult: %x CD:%x\n", CSWResult, CommandBlockWrapper.CommandData[0] );
	#endif
	// All stages of this transfer have completed.
	//Check for special cases. 
	//If test for unit ready command is given then
	//  return the CSW status byte.
	//Bit 0 == 1 == not ready else
	//Bit 0 == 0 == ready.
	//And the Start/Stop Unit command as well.
	if((CommandBlockWrapper.CommandData[0] == CMD_TEST_UNIT_READY) ||
	   (CommandBlockWrapper.CommandData[0] == CMD_START_STOP_UNIT))
		return CSWResult;
	return msProcessError(CSWResult);
}


//---------------------------------------------------------------------------
// Write Sectors (Multi Sector Capable)
uint8_t USBDrive::msWriteBlocks(
                                  const uint32_t BlockAddress,
                                  const uint16_t Blocks,
                                  const uint16_t BlockSize,
								  const void * sectorBuffer)
	{
#ifdef DBGprint
	println("msWriteBlocks()");
#endif
	uint8_t BlockHi = (Blocks >> 8) & 0xFF;
	uint8_t BlockLo = Blocks & 0xFF;
	msCommandBlockWrapper_t CommandBlockWrapper = (msCommandBlockWrapper_t)
	{
		.Signature          = CBW_SIGNATURE,
		.Tag                = ++CBWTag,
		.TransferLength     = (uint32_t)(Blocks * BlockSize),
		.Flags              = CMD_DIR_DATA_OUT,
		.LUN                = currentLUN,
		.CommandLength      = 10,
		.CommandData        = {CMD_WR_10, 0x00,
		                      (uint8_t)(BlockAddress >> 24),
							  (uint8_t)(BlockAddress >> 16),
							  (uint8_t)(BlockAddress >> 8),
							  (uint8_t)(BlockAddress & 0xFF),
							  0x00, BlockHi, BlockLo, 0x00}
	};
	#if defined(__IMXRT1062__)
	if ((uint32_t)sectorBuffer >= 0x20200000u)  arm_dcache_flush((void*)sectorBuffer, CommandBlockWrapper.TransferLength);
	#endif
	return msDoCommand(&CommandBlockWrapper, (void *)sectorBuffer);
}

// Proccess Possible SCSI errors
uint8_t USBDrive::msProcessError(uint8_t msStatus) {
#ifdef DBGprint
	println("msProcessError()");
#endif
	uint8_t msResult = 0;
	switch(msStatus) {
		case MS_CBW_PASS:
			return MS_CBW_PASS;
			break;
		case MS_CBW_PHASE_ERROR:
			print("SCSI Phase Error: ");
			println(msStatus);
			return MS_SCSI_ERROR;
			break;
		case MS_CSW_TAG_ERROR:
			print("CSW Tag Error: ");
			println(MS_CSW_TAG_ERROR);
			return MS_CSW_TAG_ERROR;
			break;
		case MS_CSW_SIG_ERROR:
			print("CSW Signature Error: ");
			println(MS_CSW_SIG_ERROR);
			return MS_CSW_SIG_ERROR;
			break;
		case MS_CBW_FAIL:
			if((msResult = msRequestSense(&msSense))) {
				print("Failed to get sense codes. Returned code: ");
				println(msResult);
			}
			return MS_CBW_FAIL;
			break;
		default:
			print("SCSI Error: ");
			println(msStatus);
			return msStatus;
	}
}



#include "mscSenseKeyList.h"


//==============================================================================

bool USBDrive::begin() {
	m_errorCode = MS_CBW_PASS;
	
	//mscInit(); // Do initial init of each instance of a MSC object.

	m_errorCode = checkConnectedInitialized();
	if (m_errorCode) { // Check for Connected USB drive.
		m_initDone = false;
	} else {
		m_initDone = true;
	}
	return m_initDone;
}

//------------------------------------------------------------------------------
bool USBDrive::readSector(uint32_t sector, uint8_t* dst) {
	return readSectors(sector, dst, 1);
}
//------------------------------------------------------------------------------
bool USBDrive::readSectors(uint32_t sector, uint8_t* dst, size_t n) {
	// Check if device is plugged in and initialized
	m_errorCode = checkConnectedInitialized();
	if (m_errorCode != MS_CBW_PASS) {
		return false;
	}
	m_errorCode = msReadBlocks(sector, n, (uint16_t)msDriveInfo.capacity.BlockSize, dst);
	if (m_errorCode) {
		return false;
	}
	return true;
}

//------------------------------------------------------------------------------
bool USBDrive::readSectorsWithCB(uint32_t sector, size_t ns,
	void (*callback)(uint32_t, uint8_t *), uint32_t token)
{
	// Check if device is plugged in and initialized
	m_errorCode = checkConnectedInitialized();
	if (m_errorCode != MS_CBW_PASS) {
		return false;
	}
	m_errorCode = msReadSectorsWithCB(sector, ns, callback, token);
	if (m_errorCode) {
		return false;
	}
	return true;
}
//------------------------------------------------------------------------------
static void callback_shim(uint32_t token, uint8_t *data)
{
	uint32_t *state = (uint32_t *)token;
	uint32_t sector = state[0];
	void (*callback)(uint32_t, uint8_t *, void *) =
		(void (*)(uint32_t, uint8_t *, void *))(state[1]);
	void *context = (void *)(state[2]);
	callback(sector, data, context);
	state[0]++;
}
bool USBDrive::readSectorsCallback(uint32_t sector, uint8_t* dst, size_t numSectors,
	void (*callback)(uint32_t sector, uint8_t *buf, void *context), void *context)
{
	uint32_t state[3] = {sector, (uint32_t)callback, (uint32_t)context};
	return readSectorsWithCB(sector, numSectors, callback_shim, (uint32_t)state);
}
//------------------------------------------------------------------------------
bool USBDrive::writeSector(uint32_t sector, const uint8_t* src) {
	return writeSectors(sector, src, 1);
}
//------------------------------------------------------------------------------
bool USBDrive::writeSectors(uint32_t sector, const uint8_t* src, size_t n) {
	// Check if device is plugged in and initialized
	m_errorCode = checkConnectedInitialized();
	if (m_errorCode != MS_CBW_PASS) {
		return false;
	}
	m_errorCode = msWriteBlocks(sector, n, (uint16_t)msDriveInfo.capacity.BlockSize, src);
	if (m_errorCode) {
		return false;
	}
	return true;
}


static const char *decodeSenseKey(uint8_t senseKey) {
	static char msg[64];
#undef SENSE_KEY_MAP
	switch (senseKey) {
#define SENSE_KEY_MAP(_name_, _val_) \
		case _val_: return #_name_ ;
		SENSE_KEY_LIST
	}
#undef SENSE_KEY_MAP

	snprintf(msg, sizeof(msg), "UNKNOWN SENSE KEY(%02Xh)", senseKey);
	return msg;
}

static const char *decodeAscAscq(uint8_t asc, uint8_t ascq) {
	static char msg[64];
	uint16_t ascAscq = asc<<8 | ascq;

	switch (ascAscq) {
#define SENSE_CODE_KEYED(_asc_, _fmt_)
#define SENSE_CODE(_asc_, _ascq_, _msg_) case _asc_<<8 | _ascq_: return _msg_;
	ASC_NUM_LIST
#undef SENSE_CODE
#undef SENSE_CODE_KEYED
	}

#define SENSE_CODE_KEYED(_asc_, _fmt_) if (asc == _asc_) { snprintf(msg, sizeof(msg), _fmt_, ascq); return msg; }
#define SENSE_CODE(_asc_, _ascq_, _msg_)
	ASC_NUM_LIST
#undef SENSE_CODE
#undef SENSE_CODE_KEYED

	snprintf(msg, sizeof(msg), "UNKNOWN ASC/ASCQ (%02Xh/%02Xh)", asc, ascq);
	return msg;
}

//------------------------------------------------------------------------------
static void printMscAscError(print_t* pr, USBDrive *pDrive)
{
	#ifdef MASS_STORAGE_PRINT_DEBUG 
	Serial.printf(" --> Type: %s Cause: %s\n",
		decodeSenseKey(pDrive->msSense.SenseKey),
		decodeAscAscq(pDrive->msSense.AdditionalSenseCode,
		pDrive->msSense.AdditionalSenseQualifier));
	#endif
}

#undef print
#undef println

// Print error info and return.
//


void USBDrive::printPartionTable(Print &p) {
  DBGPrintf(">>USBDrive::printPartionTable\n");
  if (!msDriveInfo.initialized) return;
  const uint32_t device_sector_count = msDriveInfo.capacity.Blocks;
  // TODO: check device_sector_count
  MbrSector_t mbr;
  bool gpt_disk = false;
  bool ext_partition;
  uint32_t next_free_sector = 8192;  // Some inital value this is default for Win32 on SD...
  if (!readSector(0, (uint8_t*)&mbr)) {
    p.printf("\nread MBR failed, error code 0x%02X.\n", errorCode());
    return;
  }
  p.print("\nPartition Table\n");
  p.print("\tpart,boot,bgnCHS[3],type,endCHS[3],start,length\n");
  for (uint8_t ip = 1; ip < 5; ip++) {
    MbrPart_t *pt = &mbr.part[ip - 1];
    uint32_t starting_sector = getLe32(pt->relativeSectors);
    uint32_t total_sector = getLe32(pt->totalSectors);
    ext_partition = false;
    if (starting_sector > next_free_sector) {
      p.printf("\t < unused area starting at: %u length %u >\n", next_free_sector, starting_sector-next_free_sector);
    }
    switch (pt->type) {
    case 1:
      p.print("FAT12:\t");
      break;
    case 4:
    case 6:
    case 0xe:
      p.print("FAT16:\t");
      break;
    case 11:
    case 12:
      p.print("FAT32:\t");
      break;
    case 7:
      p.print("exFAT:\t");
      break;
    case 5:
    case 0xf:
      p.print("Extend:\t");
      ext_partition = true;
      break;
    case 0x83:
      p.print("ext2/3/4:\t");
      break;
    case 0xee:
      p.print(F("*** GPT Disk WIP ***\nGPT guard:\t"));
      gpt_disk = true;
      break;
    default:
      p.print("pt_#");
      p.print(pt->type);
      p.print(":\t");
      break;
    }
    p.print( int(ip)); p.print( ',');
    p.print(int(pt->boot), HEX); p.print( ',');
    for (int i = 0; i < 3; i++ ) {
      p.print("0x"); p.print(int(pt->beginCHS[i]), HEX); p.print( ',');
    }
    p.print("0x"); p.print(int(pt->type), HEX); p.print( ',');
    for (int i = 0; i < 3; i++ ) {
      p.print("0x"); p.print(int(pt->endCHS[i]), HEX); p.print( ',');
    }
    p.print(starting_sector, DEC); p.print(',');
    p.println(total_sector);
    if (ext_partition) {
      printExtendedPartition(&mbr, ip, p);
      readSector(0, (uint8_t*)&mbr); // maybe need to restore
    }

    // Lets get the max of start+total
    if (starting_sector && total_sector)  next_free_sector = starting_sector + total_sector;
  }
  if (next_free_sector < device_sector_count) {
    p.printf("\t < unused area starting at: %u length %u >\n",
      next_free_sector, device_sector_count-next_free_sector);
  }
  if (gpt_disk) printGUIDPartitionTable(p);
}

void dump_hexbytes(const void *ptr, int len, Print &pr)
{
  if (ptr == NULL || len <= 0) return;
  const uint8_t *p = (const uint8_t *)ptr;
  while (len > 0) {
    for (uint8_t i = 0; i < 32; i++) {
      if (i > len) break;
      pr.printf("%02X ", p[i]);
    }
    pr.print(":");
    for (uint8_t i = 0; i < 32; i++) {
      if (i > len) break;
      pr.printf("%c", ((p[i] >= ' ') && (p[i] <= '~')) ? p[i] : '.');
    }
    pr.println();
    p += 32;
    len -= 32;
  }
}

void USBDrive::printExtendedPartition(MbrSector_t *mbr, uint8_t ipExt, Print &p) {
  // Extract the data from EX partition block...
  MbrPart_t *pt = &mbr->part[ipExt - 1];
  uint32_t ext_starting_sector = getLe32(pt->relativeSectors);
  //uint32_t ext_total_sector = getLe32(pt->totalSectors);
  uint32_t next_mbr = ext_starting_sector;
  uint8_t ext_index = 0;

  while (next_mbr) {
    ext_index++;
    if (!readSector(next_mbr, (uint8_t*)mbr)) break;
    pt = &mbr->part[0];
    //dump_hexbytes((uint8_t*)pt, sizeof(MbrPart_t)*2, p);
    uint32_t starting_sector = getLe32(pt->relativeSectors);
    uint32_t total_sector = getLe32(pt->totalSectors);
    switch (pt->type) {
    case 1:
      p.print(F("FAT12:\t"));
      break;
    case 4:
    case 6:
    case 0xe:
      p.print(F("FAT16:\t"));
      break;
    case 11:
    case 12:
      p.print(F("FAT32:\t"));
      break;
    case 7:
      p.print(F("exFAT:\t"));
      break;
    case 0xf:
      p.print(F("Extend:\t"));
      break;
    case 0x83:
      p.print(F("ext2/3/4:\t")); break;
    default:
      p.print(F("pt_#"));
      p.print(pt->type);
      p.print(":\t");
      break;
    }
    // TODO: extended partition numbers increment from 5
    p.print( int(ipExt)); p.print(":"); p.print(ext_index); p.print( ',');
    p.print(int(pt->boot), HEX); p.print( ',');
    for (int i = 0; i < 3; i++ ) {
      p.print("0x"); p.print(int(pt->beginCHS[i]), HEX); p.print( ',');
    }
    p.print("0x"); p.print(int(pt->type), HEX); p.print( ',');
    for (int i = 0; i < 3; i++ ) {
      p.print("0x"); p.print(int(pt->endCHS[i]), HEX); p.print( ',');
    }
    p.printf("%u(%u),", next_mbr + starting_sector, starting_sector);
    //p.print(ext_starting_sector + starting_sector, DEC); p.print(',');
    p.print(total_sector);

    // Now lets see what is in the 2nd one...
    pt = &mbr->part[1];
    p.printf(" (%x)\n", pt->type);
    starting_sector = getLe32(pt->relativeSectors);
    if (pt->type && starting_sector) next_mbr = starting_sector + ext_starting_sector;
    else next_mbr = 0;
  }
}

#if 0
typedef struct {
  uint8_t  signature[8];
  uint8_t  revision[4];
  uint8_t  headerSize[4];
  uint8_t  crc32[4];
  uint8_t  reserved[4];
  uint8_t  currentLBA[8];
  uint8_t  backupLBA[8];
  uint8_t  firstLBA[8];
  uint8_t  lastLBA[8];
  uint8_t  diskGUID[16];
  uint8_t  startLBAArray[8];
  uint8_t  numberPartitions[4];
  uint8_t  sizePartitionEntry[4];
  uint8_t  crc32PartitionEntries[4];
  uint8_t  unused[420]; // should be 0;
} GPTPartitionHeader_t;

typedef struct {
  uint8_t  partitionTypeGUID[16];
  uint8_t  uniqueGUID[16];
  uint8_t  firstLBA[8];
  uint8_t  lastLBA[8];
  uint8_t  attributeFlags[8];
  uint16_t name[36];
} GPTPartitionEntryItem_t;

typedef struct {
  GPTPartitionEntryItem_t items[4];
} GPTPartitionEntrySector_t;
#endif

typedef struct {
  uint32_t  q1;
  uint16_t  w2;
  uint16_t  w3;
  uint8_t   b[8];
} guid_t;


void printGUID(uint8_t* pbguid, Print &p) {
  // Windows basic partion guid is: EBD0A0A2-B9E5-4433-87C0-68B6B72699C7
  // raw dump of it: A2 A0 D0 EB E5 B9 33 44 87 C0 68 B6 B7 26 99 C7
  guid_t *pg = (guid_t*)pbguid;
  p.printf("%08X-%04X-%04X-%02X%02X-", pg->q1, pg->w2, pg->w3, pg->b[0], pg->b[1]);
  for (uint8_t i=2;i<8; i++) p.printf("%02X", pg->b[i]);
}

uint32_t USBDrive::printGUIDPartitionTable(Print &Serialx) {
  union {
    MbrSector_t mbr;
    partitionBootSector pbs;
    GPTPartitionHeader_t gpthdr;
    GPTPartitionEntrySector_t gptes;
    uint8_t buffer[512];
  } sector;

  // Lets verify that we are an GPT...
  if (!readSector(0, (uint8_t*)&sector.mbr)) {
    Serialx.print(F("\nread MBR failed.\n"));
    //errorPrint();
    return (uint32_t)-1;
  }
  // verify that the first partition is the guard...
  MbrPart_t *pt = &sector.mbr.part[0];
  if (pt->type != 0xee) {
    Serialx.print(F("\nMBR is not an gpt guard\n"));
    return (uint32_t)-1;
  }

  if (!readSector(1, (uint8_t*)&sector.buffer)) {
    Serialx.print(F("\nread Partition Table Header failed.\n"));
    return (uint32_t)-1;
  }
  // Do quick test for signature:
  if (memcmp(sector.gpthdr.signature, "EFI PART", 8)!= 0) {
    Serialx.println("GPT partition header signature did not match");
    dump_hexbytes(&sector.buffer, 512, Serialx);
  }
  Serialx.printf("\nGPT partition header revision: %x\n", getLe32(sector.gpthdr.revision));
  Serialx.printf("LBAs current:%llu backup:%llu first:%llu last:%llu\nDisk GUID:",
    getLe64(sector.gpthdr.currentLBA), getLe64(sector.gpthdr.backupLBA),
    getLe64(sector.gpthdr.firstLBA), getLe64(sector.gpthdr.lastLBA));
  printGUID(sector.gpthdr.diskGUID, Serialx);

  //dump_hexbytes(&sector.gpthdr.diskGUID, 16);
  uint32_t cParts = getLe32(sector.gpthdr.numberPartitions);
  Serialx.printf("Start LBA Array: %llu Count: %u size:%u\n",
      getLe64(sector.gpthdr.startLBAArray), cParts, getLe32(sector.gpthdr.sizePartitionEntry));
  uint32_t sector_number = 2;
  Serialx.println("Part\t Type Guid, Unique Guid, First, last, attr, name");
  for (uint8_t part = 0; part < cParts ; part +=4) {
    if (readSector(sector_number, (uint8_t*)&sector.buffer)) {
      //dump_hexbytes(&sector.buffer, 512);
      for (uint8_t ipei = 0; ipei < 4; ipei++) {
        GPTPartitionEntryItem_t *pei = &sector.gptes.items[ipei];
        // see if the entry has any data in it...
        uint32_t end_addr = (uint32_t)pei + sizeof(GPTPartitionEntryItem_t);
        uint32_t *p = (uint32_t*)pei;
        for (; (uint32_t)p < end_addr; p++) {
          if (*p) break; // found none-zero.
        }
        if ((uint32_t)p < end_addr) {
          // So entry has data:
          Serialx.printf("%u\t", part + ipei);
          printGUID(pei->partitionTypeGUID, Serialx);
          Serialx.print(", ");
          printGUID(pei->uniqueGUID, Serialx);
          Serialx.printf(", %llu, %llu, %llX, ", getLe64(pei->firstLBA), getLe64(pei->lastLBA),
              getLe64(pei->attributeFlags));
          for (uint8_t i = 0; i < 36; i++) {
            if ((pei->name[i]) == 0) break;
            Serialx.write((uint8_t)pei->name[i]);
          }
          Serialx.println();
          if (memcmp((uint8_t *)pei->partitionTypeGUID, mbdpGuid, 16) == 0) {
            Serialx.print(">>> Microsoft Basic Data Partition\n");
            // See if we can read in the first sector
            if (readSector(getLe64(pei->firstLBA), (uint8_t*)&sector.buffer)) {
              //dump_hexbytes(sector.buffer, 512);

              // First see if this is exFat...
              // which starts with:
              static const uint8_t exfatPBS[] PROGMEM = {0xEB, 0x76, 0x90, //Jmp instruction
                   'E', 'X', 'F', 'A', 'T', ' ', ' ', ' '};
              if (memcmp(sector.buffer, exfatPBS, 11) == 0) {
				  #ifdef MASS_STORAGE_PRINT_DEBUG
                Serial.println("    EXFAT:");
				#endif
              }

            }
            // Bugbug reread that sector...
            readSector(sector_number, (uint8_t*)&sector.buffer);
          }
        }
      }
    }
    sector_number++;
  }
  return 0;
}


//=============================================================================
// FindPartition - 
//=============================================================================
int USBDrive::findPartition(int partition, int &type, uint32_t &firstSector, uint32_t &numSectors, 
							uint32_t &mbrLBA, uint8_t &mbrPart, uint8_t *guid)
{
	if (partition == 0) {
		type = 6; // assume whole drive is FAT16 (SdFat will detect actual format)
		firstSector = 0;
		numSectors = msDriveInfo.capacity.Blocks;
		return MBR_VOL;
	}
	union {
		MbrSector_t mbr;
		partitionBootSector pbs;
		GPTPartitionHeader_t gpthdr;
		GPTPartitionEntrySector_t gptes;
		uint8_t buffer[512];
	} sector;


	partition--;  // zero bias it. 
	if (!readSector(0, (uint8_t*)&sector.mbr)) return INVALID_VOL;
	MbrPart_t *pt = &sector.mbr.part[0];
	if (pt->type == 0xee) {
		// See if we have already cached number of partitions
		if (_cGPTParts == 0) {
			if (!readSector(1, (uint8_t*)&sector.buffer)) return INVALID_VOL;
	  		_cGPTParts = (int)getLe32(sector.gpthdr.numberPartitions);
	  		DBGPrintf(">>Find Partition GPT cParts=%d\n", _cGPTParts);
		}
		// GUID Partition Table
		//  TODO: should we read sector 1, check # of entries and entry size = 128?
		if (partition >= _cGPTParts) return INVALID_VOL; // ran off end
		mbrLBA = 2 + (partition >> 2);
		mbrPart = partition & 0x3;
		if (!readSector(mbrLBA, (uint8_t*)&sector.mbr)) return INVALID_VOL;

		GPTPartitionEntryItem_t *entry = &sector.gptes.items[mbrPart];
		// if we have an empty item we figure we are done.
        uint32_t *end_addr = (uint32_t*)((uint32_t)entry + sizeof(GPTPartitionEntryItem_t));
        uint32_t *p = (uint32_t*)entry;
        for (; p < end_addr; p++) {
          if (*p) break; // found none-zero.
        }
        
       	if (p < end_addr) {
			uint64_t first64 = getLe64(entry->firstLBA);
			if (first64 > 0x00000000FFFFFFFFull) return INVALID_VOL;
			uint32_t first32 = first64;
			uint64_t last64 = getLe64(entry->lastLBA);
			if (last64 > 0x00000000FFFFFFFFull) return INVALID_VOL;
			uint32_t last32 = last64;
			if (first32 > last32) return INVALID_VOL;
			firstSector = first32;
			numSectors = last32 - first32 + 1;
			// bugbug should be caller that knows which guids they deal with.
			// Not sure if I hould try to remove this yet, or if
			// we may want to extend list of guids if others are found
			// we understatnd
			if (guid) memcpy(guid, entry->partitionTypeGUID, 16);
			type = 6;
			return GPT_VOL;
		}
   		return INVALID_VOL;
	}
	if (partition >= 0 && partition <= 3) {
		// Master Boot Record
		pt = &sector.mbr.part[partition];
        // try quick way through
      	if (((pt->boot == 0) || (pt->boot == 0X80)) && (pt->type != 0) && (pt->type != 0xf)) {
			type = pt->type;
			firstSector = getLe32(pt->relativeSectors);
			numSectors = getLe32(pt->totalSectors);
			mbrLBA = 0;
			mbrPart = partition; // zero based
			return MBR_VOL;
		}
	}

    // So must be extended or invalid.
    uint8_t index_part;
    for (index_part = 0; index_part < 4; index_part++) {
      pt = &sector.mbr.part[index_part];
      if ((pt->boot != 0 && pt->boot != 0X80) || pt->type == 0 || index_part > partition) return INVALID_VOL;
      if (pt->type == 0xf) break;
    }

    if (index_part == 4) return INVALID_VOL; // no extended partition found. 

    // Our partition if it exists is in extended partition. 
    uint32_t next_mbr = getLe32(pt->relativeSectors);
    for(;;) {
	  if (!readSector(next_mbr, (uint8_t*)&sector.mbr)) return INVALID_VOL;

      if (index_part == partition) break; // should be at that entry
      // else we need to see if it points to others...
      pt = &sector.mbr.part[1];
      uint32_t  relSec = getLe32(pt->relativeSectors);
      //Serial.printf("    Check for next: type: %u start:%u\n ", pt->type, volumeStartSector);
      if ((pt->type == 5) && relSec) {
        next_mbr = next_mbr + relSec;
        index_part++; 
      } else return INVALID_VOL;
    }
   
    // If we are here than we should hopefully be at start of segment...
    pt = &sector.mbr.part[0];
	type = pt->type;
	firstSector = getLe32(pt->relativeSectors) + next_mbr;
	numSectors = getLe32(pt->totalSectors);
	mbrLBA = next_mbr;
	mbrPart = 0; // zero based
    return EXT_VOL;
  }



//=============================================================================
// startFilesystems - enumerate all of the partitons of a drive and ask the different
// filesystem objects if they would like to claim the partition. 
// returns - true if our enumeration  has any partitions claimed. 
//=============================================================================
bool USBDrive::startFilesystems()
{
	// first repeat calling findPartition()
	int type;
	uint32_t firstSector;
	uint32_t numSectors;
	int voltype;
	bool file_system_claimed = false;
	uint32_t mbrLBA;
	uint8_t mbrPart;

	uint8_t guid[16];

	DBGPrintf(">> USBDrive::startFilesystems called %p\n", this);

	if (!begin()) { // make sure we are initialized
		DBGPrintf("\t >> begin() failed");
		return false;
	}

	for (int part = 1; ;part++) {
		voltype = findPartition(part, type, firstSector, numSectors, mbrLBA, mbrPart, guid);
		if (voltype == INVALID_VOL) break;
		DBGPrintf("\t>>Partition %d VT:%u T:%U %u %u\n", part, voltype, type, firstSector, numSectors);
		// Now see if there is any file systems that wish to claim this partition.
		 USBFSBase *usbfsPrev = nullptr;
		 USBFSBase *usbfs = available_filesystem_list;

		 while (usbfs) {
		 	if (usbfs->claimPartition(this, part, voltype, type, firstSector, numSectors, guid)) break;
		 	usbfsPrev = usbfs;
		 	usbfs = usbfs->next;
		 }
		 if (usbfs) {
		 	// unlink
		 	if (usbfsPrev) usbfsPrev->next = usbfs->next;
		 	else available_filesystem_list = usbfs->next;

		 	// link to this drives list of claimed
		 	usbfs->next = claimed_filesystem_list;
		 	claimed_filesystem_list = usbfs;

		 	// and put a link to us 
		 	usbfs->mydevice = device;
		 	file_system_claimed = true;
		 }

		}
	_filesystems_started = true;
	return file_system_claimed;
} 

//=============================================================================
// USBFileSystem methods
//=============================================================================

void USBFilesystem::init()
{
	USBDrive::filesystem_ready_for_drive(this);
}

FLASHMEM
void USBFilesystem::printError(Print* p) {
	const uint8_t err = device->errorCode();
	if (err && p) {
		if (err == 0x28) {
			p->println(F("No USB drive detected, plugged in?"));
		}
		p->print(F("USB drive error: "));
		p->print(F("0x"));
		p->print(err, HEX);
		p->print(F(",0x"));
		p->print(device->errorData(), HEX);
		printMscAscError(p, device);
	} else if (!mscfs.fatType()) {
		p->println(F("Check USB drive format."));
	}
}


// We only support a limited number of GUIDS (currently 1)
bool USBFilesystem::check_voltype_guid(int voltype, uint8_t *guid) {
	// Microsoft Basic Data Partition
	DBGPrintf(">>USBFilesystem::check_voltype_guid(%d, %p)\n", voltype, guid);
	if (voltype == USBDrive::GPT_VOL) {
		#ifdef DBGprint
		printGUID(guid, Serial);
		#endif
		if (memcmp(guid, mbdpGuid, 16) == 0) return true;
		DBGPrintf("USBFilesystem - Unsupporteded GUID\n");
		return false;
	}
	return true;
}

//=============================================================================
// USBFilesystem::begin - Manual way to startup a filesystem object
//=============================================================================

bool USBFilesystem::begin(USBDrive *pDrive, bool setCwv, uint8_t part) {
	DBGPrintf(">>USBFilesystem::begin(%p, %u, %u)\n", pDrive, setCwv, part); DBGFlush();

	if (device) return false;  // object is already in use. 
	uint8_t guid[16];
	device = pDrive;
	device->begin();

	if (device->errorCode() != 0) return false;
	//device->printPartionTable(Serial);
	int type;
	uint32_t firstSector, numSectors;
	uint32_t mbrLBA;
	uint8_t mbrPart;


	int voltype = device->findPartition(part, type, firstSector, numSectors, mbrLBA, mbrPart, guid);
	if (!voltype) {
		device = nullptr;
		return false;  // not a valid volume...
	}

	// if this is a GPT setup, then make sure the guid is one we want.
	if (!check_voltype_guid(voltype, guid)) {
		device = nullptr;
		return false;
	}

	if (mscfs.begin(pDrive, setCwv, firstSector, numSectors)) {
		device->filesystem_assign_to_drive(this, true);
	}

	

	return true;
}

void USBFilesystem::end(bool update_list) {
	mscfs.end();
	if (update_list) device->filesystem_assign_to_drive(this, false);
	device = nullptr;
}


bool USBFilesystem::claimPartition(USBDrive *pdevice, int part,int voltype, int type, uint32_t firstSector, uint32_t numSectors, uint8_t *guid) {
	// May add in some additional stuff
	DBGPrintf("\t>>USBFilesystem::claimPartition %p called ");

	// For GUID file systems only continue if this is a guid to a type we know. 
	if (!check_voltype_guid(voltype, guid)) return false; // not something we understand;

	if (mscfs.begin(pdevice, true, firstSector, numSectors)) {
		device = pdevice;
		partition = part;
		partitionType = type;
		DBGPrintf("+ Claimed\n");
		return true;		
	}
	DBGPrintf("- Not Claimed\n");
	return false;
}

void USBFilesystem::releasePartition() {
	DBGPrintf("\t USBFilesystem::releasePartition %p called\n");
	end(false);
}

bool USBFilesystem::format(int type, char progressChar, Print* pr) {
	// setup instance of formatter object;
	uint8_t *buf = (uint8_t *)malloc(512+32);
	if (!buf) return false; // unable to allocate memory
	// lets align the buffer
    uint8_t *aligned_buf = (uint8_t *)(((uintptr_t)buf + 31) & ~((uintptr_t)(31)));
	USBFilesystemFormatter formatter; 
	#ifdef MASS_STORAGE_PRINT_DEBUG
	Serial.printf("$$call formatter.format(%p, 0, %p %p...)\n", this, buf, aligned_buf);
	#endif
	bool ret = formatter.format(*this, 0, aligned_buf, pr);

	free(buf);

	if (ret) {
		if (pr) pr->println("Format Completed restart filesystem");
		
		// Maybe not call as this may write out dirty stuff.
		//mscfs.end();  // release the old data

		// Now lets try to restart it	
		int type;
		uint32_t firstSector;
		uint32_t numSectors;
		uint32_t mbrLBA;
		uint8_t mbrPart;

		uint8_t guid[16];

		int voltype = device->findPartition(partition, type, firstSector, numSectors, mbrLBA, mbrPart, guid);
		if (voltype == USBDrive::INVALID_VOL) return false;
		if (pr) pr->printf("\tPart:%d Type:%x First:%u num:%u\n", partition, type, firstSector, numSectors);
		// now lets try to start it again.
		partitionType = type;
		ret = mscfs.begin(device, true, firstSector, numSectors);
		if (pr) pr->printf("\tbegin return: %u\n", ret);
		changed(true);  // mark it as changed.
	}
	return ret;
}
 
I've been looking around at some other USB_Host stuff and it occurs to me, that we should be able to rewrite the begin() method as a state machine so that it's non-block. begin() could be a void method signals to the driver to try an open the drive in the background, later setting a flag if it was successful or not. I'll experiment with it next week.
 
I've been looking around at some other USB_Host stuff and it occurs to me, that we should be able to rewrite the begin() method as a state machine so that it's non-block. begin() could be a void method signals to the driver to try an open the drive in the background, later setting a flag if it was successful or not. I'll experiment with it next week.

I'm playing around with TeensyThreads.h. Looks the perfect companion to this library if you want non-blocking functionality.
 
Back
Top