Merge commit '60b30c036397cb5627fa374bb930794b225daa29' as 'lib/lufa'

4 files changed, 1059 insertions, 0 deletions

diff --git a/lib/lufa/Demos/Device/ClassDriver/MassStorageKeyboard/Lib/DataflashManager.c b/lib/lufa/Demos/Device/ClassDriver/MassStorageKeyboard/Lib/DataflashManager.c
new file mode 100644
index 0000000000..c0fd16ccc4
--- /dev/null
+++ b/lib/lufa/Demos/Device/ClassDriver/MassStorageKeyboard/Lib/DataflashManager.c
@@ -0,0 +1,534 @@
+/*
+             LUFA Library
+     Copyright (C) Dean Camera, 2017.
+
+  dean [at] fourwalledcubicle [dot] com
+           www.lufa-lib.org
+*/
+
+/*
+  Copyright 2017  Dean Camera (dean [at] fourwalledcubicle [dot] com)
+
+  Permission to use, copy, modify, distribute, and sell this
+  software and its documentation for any purpose is hereby granted
+  without fee, provided that the above copyright notice appear in
+  all copies and that both that the copyright notice and this
+  permission notice and warranty disclaimer appear in supporting
+  documentation, and that the name of the author not be used in
+  advertising or publicity pertaining to distribution of the
+  software without specific, written prior permission.
+
+  The author disclaims all warranties with regard to this
+  software, including all implied warranties of merchantability
+  and fitness.  In no event shall the author be liable for any
+  special, indirect or consequential damages or any damages
+  whatsoever resulting from loss of use, data or profits, whether
+  in an action of contract, negligence or other tortious action,
+  arising out of or in connection with the use or performance of
+  this software.
+*/
+
+/** \file
+ *
+ *  Functions to manage the physical Dataflash media, including reading and writing of
+ *  blocks of data. These functions are called by the SCSI layer when data must be stored
+ *  or retrieved to/from the physical storage media. If a different media is used (such
+ *  as a SD card or EEPROM), functions similar to these will need to be generated.
+ */
+
+#define  INCLUDE_FROM_DATAFLASHMANAGER_C
+#include "DataflashManager.h"
+
+/** Writes blocks (OS blocks, not Dataflash pages) to the storage medium, the board Dataflash IC(s), from
+ *  the pre-selected data OUT endpoint. This routine reads in OS sized blocks from the endpoint and writes
+ *  them to the Dataflash in Dataflash page sized blocks.
+ *
+ *  \param[in] MSInterfaceInfo  Pointer to a structure containing a Mass Storage Class configuration and state
+ *  \param[in] BlockAddress  Data block starting address for the write sequence
+ *  \param[in] TotalBlocks   Number of blocks of data to write
+ */
+void DataflashManager_WriteBlocks(USB_ClassInfo_MS_Device_t* const MSInterfaceInfo,
+                                  const uint32_t BlockAddress,
+                                  uint16_t TotalBlocks)
+{
+	uint16_t CurrDFPage          = ((BlockAddress * VIRTUAL_MEMORY_BLOCK_SIZE) / DATAFLASH_PAGE_SIZE);
+	uint16_t CurrDFPageByte      = ((BlockAddress * VIRTUAL_MEMORY_BLOCK_SIZE) % DATAFLASH_PAGE_SIZE);
+	uint8_t  CurrDFPageByteDiv16 = (CurrDFPageByte >> 4);
+	bool     UsingSecondBuffer   = false;
+
+	/* Select the correct starting Dataflash IC for the block requested */
+	Dataflash_SelectChipFromPage(CurrDFPage);
+
+#if (DATAFLASH_PAGE_SIZE > VIRTUAL_MEMORY_BLOCK_SIZE)
+	/* Copy selected dataflash's current page contents to the Dataflash buffer */
+	Dataflash_SendByte(DF_CMD_MAINMEMTOBUFF1);
+	Dataflash_SendAddressBytes(CurrDFPage, 0);
+	Dataflash_WaitWhileBusy();
+#endif
+
+	/* Send the Dataflash buffer write command */
+	Dataflash_SendByte(DF_CMD_BUFF1WRITE);
+	Dataflash_SendAddressBytes(0, CurrDFPageByte);
+
+	/* Wait until endpoint is ready before continuing */
+	if (Endpoint_WaitUntilReady())
+	  return;
+
+	while (TotalBlocks)
+	{
+		uint8_t BytesInBlockDiv16 = 0;
+
+		/* Write an endpoint packet sized data block to the Dataflash */
+		while (BytesInBlockDiv16 < (VIRTUAL_MEMORY_BLOCK_SIZE >> 4))
+		{
+			/* Check if the endpoint is currently empty */
+			if (!(Endpoint_IsReadWriteAllowed()))
+			{
+				/* Clear the current endpoint bank */
+				Endpoint_ClearOUT();
+
+				/* Wait until the host has sent another packet */
+				if (Endpoint_WaitUntilReady())
+				  return;
+			}
+
+			/* Check if end of Dataflash page reached */
+			if (CurrDFPageByteDiv16 == (DATAFLASH_PAGE_SIZE >> 4))
+			{
+				/* Write the Dataflash buffer contents back to the Dataflash page */
+				Dataflash_WaitWhileBusy();
+				Dataflash_SendByte(UsingSecondBuffer ? DF_CMD_BUFF2TOMAINMEMWITHERASE : DF_CMD_BUFF1TOMAINMEMWITHERASE);
+				Dataflash_SendAddressBytes(CurrDFPage, 0);
+
+				/* Reset the Dataflash buffer counter, increment the page counter */
+				CurrDFPageByteDiv16 = 0;
+				CurrDFPage++;
+
+				/* Once all the Dataflash ICs have had their first buffers filled, switch buffers to maintain throughput */
+				if (Dataflash_GetSelectedChip() == DATAFLASH_CHIP_MASK(DATAFLASH_TOTALCHIPS))
+				  UsingSecondBuffer = !(UsingSecondBuffer);
+
+				/* Select the next Dataflash chip based on the new Dataflash page index */
+				Dataflash_SelectChipFromPage(CurrDFPage);
+
+#if (DATAFLASH_PAGE_SIZE > VIRTUAL_MEMORY_BLOCK_SIZE)
+				/* If less than one Dataflash page remaining, copy over the existing page to preserve trailing data */
+				if ((TotalBlocks * (VIRTUAL_MEMORY_BLOCK_SIZE >> 4)) < (DATAFLASH_PAGE_SIZE >> 4))
+				{
+					/* Copy selected dataflash's current page contents to the Dataflash buffer */
+					Dataflash_WaitWhileBusy();
+					Dataflash_SendByte(UsingSecondBuffer ? DF_CMD_MAINMEMTOBUFF2 : DF_CMD_MAINMEMTOBUFF1);
+					Dataflash_SendAddressBytes(CurrDFPage, 0);
+					Dataflash_WaitWhileBusy();
+				}
+#endif
+
+				/* Send the Dataflash buffer write command */
+				Dataflash_SendByte(UsingSecondBuffer ? DF_CMD_BUFF2WRITE : DF_CMD_BUFF1WRITE);
+				Dataflash_SendAddressBytes(0, 0);
+			}
+
+			/* Write one 16-byte chunk of data to the Dataflash */
+			Dataflash_SendByte(Endpoint_Read_8());
+			Dataflash_SendByte(Endpoint_Read_8());
+			Dataflash_SendByte(Endpoint_Read_8());
+			Dataflash_SendByte(Endpoint_Read_8());
+			Dataflash_SendByte(Endpoint_Read_8());
+			Dataflash_SendByte(Endpoint_Read_8());
+			Dataflash_SendByte(Endpoint_Read_8());
+			Dataflash_SendByte(Endpoint_Read_8());
+			Dataflash_SendByte(Endpoint_Read_8());
+			Dataflash_SendByte(Endpoint_Read_8());
+			Dataflash_SendByte(Endpoint_Read_8());
+			Dataflash_SendByte(Endpoint_Read_8());
+			Dataflash_SendByte(Endpoint_Read_8());
+			Dataflash_SendByte(Endpoint_Read_8());
+			Dataflash_SendByte(Endpoint_Read_8());
+			Dataflash_SendByte(Endpoint_Read_8());
+
+			/* Increment the Dataflash page 16 byte block counter */
+			CurrDFPageByteDiv16++;
+
+			/* Increment the block 16 byte block counter */
+			BytesInBlockDiv16++;
+
+			/* Check if the current command is being aborted by the host */
+			if (MSInterfaceInfo->State.IsMassStoreReset)
+			  return;
+		}
+
+		/* Decrement the blocks remaining counter */
+		TotalBlocks--;
+	}
+
+	/* Write the Dataflash buffer contents back to the Dataflash page */
+	Dataflash_WaitWhileBusy();
+	Dataflash_SendByte(UsingSecondBuffer ? DF_CMD_BUFF2TOMAINMEMWITHERASE : DF_CMD_BUFF1TOMAINMEMWITHERASE);
+	Dataflash_SendAddressBytes(CurrDFPage, 0x00);
+	Dataflash_WaitWhileBusy();
+
+	/* If the endpoint is empty, clear it ready for the next packet from the host */
+	if (!(Endpoint_IsReadWriteAllowed()))
+	  Endpoint_ClearOUT();
+
+	/* Deselect all Dataflash chips */
+	Dataflash_DeselectChip();
+}
+
+/** Reads blocks (OS blocks, not Dataflash pages) from the storage medium, the board Dataflash IC(s), into
+ *  the pre-selected data IN endpoint. This routine reads in Dataflash page sized blocks from the Dataflash
+ *  and writes them in OS sized blocks to the endpoint.
+ *
+ *  \param[in] MSInterfaceInfo  Pointer to a structure containing a Mass Storage Class configuration and state
+ *  \param[in] BlockAddress  Data block starting address for the read sequence
+ *  \param[in] TotalBlocks   Number of blocks of data to read
+ */
+void DataflashManager_ReadBlocks(USB_ClassInfo_MS_Device_t* const MSInterfaceInfo,
+                                 const uint32_t BlockAddress,
+                                 uint16_t TotalBlocks)
+{
+	uint16_t CurrDFPage          = ((BlockAddress * VIRTUAL_MEMORY_BLOCK_SIZE) / DATAFLASH_PAGE_SIZE);
+	uint16_t CurrDFPageByte      = ((BlockAddress * VIRTUAL_MEMORY_BLOCK_SIZE) % DATAFLASH_PAGE_SIZE);
+	uint8_t  CurrDFPageByteDiv16 = (CurrDFPageByte >> 4);
+
+	/* Select the correct starting Dataflash IC for the block requested */
+	Dataflash_SelectChipFromPage(CurrDFPage);
+
+	/* Send the Dataflash main memory page read command */
+	Dataflash_SendByte(DF_CMD_MAINMEMPAGEREAD);
+	Dataflash_SendAddressBytes(CurrDFPage, CurrDFPageByte);
+	Dataflash_SendByte(0x00);
+	Dataflash_SendByte(0x00);
+	Dataflash_SendByte(0x00);
+	Dataflash_SendByte(0x00);
+
+	/* Wait until endpoint is ready before continuing */
+	if (Endpoint_WaitUntilReady())
+	  return;
+
+	while (TotalBlocks)
+	{
+		uint8_t BytesInBlockDiv16 = 0;
+
+		/* Read an endpoint packet sized data block from the Dataflash */
+		while (BytesInBlockDiv16 < (VIRTUAL_MEMORY_BLOCK_SIZE >> 4))
+		{
+			/* Check if the endpoint is currently full */
+			if (!(Endpoint_IsReadWriteAllowed()))
+			{
+				/* Clear the endpoint bank to send its contents to the host */
+				Endpoint_ClearIN();
+
+				/* Wait until the endpoint is ready for more data */
+				if (Endpoint_WaitUntilReady())
+				  return;
+			}
+
+			/* Check if end of Dataflash page reached */
+			if (CurrDFPageByteDiv16 == (DATAFLASH_PAGE_SIZE >> 4))
+			{
+				/* Reset the Dataflash buffer counter, increment the page counter */
+				CurrDFPageByteDiv16 = 0;
+				CurrDFPage++;
+
+				/* Select the next Dataflash chip based on the new Dataflash page index */
+				Dataflash_SelectChipFromPage(CurrDFPage);
+
+				/* Send the Dataflash main memory page read command */
+				Dataflash_SendByte(DF_CMD_MAINMEMPAGEREAD);
+				Dataflash_SendAddressBytes(CurrDFPage, 0);
+				Dataflash_SendByte(0x00);
+				Dataflash_SendByte(0x00);
+				Dataflash_SendByte(0x00);
+				Dataflash_SendByte(0x00);
+			}
+
+			/* Read one 16-byte chunk of data from the Dataflash */
+			Endpoint_Write_8(Dataflash_ReceiveByte());
+			Endpoint_Write_8(Dataflash_ReceiveByte());
+			Endpoint_Write_8(Dataflash_ReceiveByte());
+			Endpoint_Write_8(Dataflash_ReceiveByte());
+			Endpoint_Write_8(Dataflash_ReceiveByte());
+			Endpoint_Write_8(Dataflash_ReceiveByte());
+			Endpoint_Write_8(Dataflash_ReceiveByte());
+			Endpoint_Write_8(Dataflash_ReceiveByte());
+			Endpoint_Write_8(Dataflash_ReceiveByte());
+			Endpoint_Write_8(Dataflash_ReceiveByte());
+			Endpoint_Write_8(Dataflash_ReceiveByte());
+			Endpoint_Write_8(Dataflash_ReceiveByte());
+			Endpoint_Write_8(Dataflash_ReceiveByte());
+			Endpoint_Write_8(Dataflash_ReceiveByte());
+			Endpoint_Write_8(Dataflash_ReceiveByte());
+			Endpoint_Write_8(Dataflash_ReceiveByte());
+
+			/* Increment the Dataflash page 16 byte block counter */
+			CurrDFPageByteDiv16++;
+
+			/* Increment the block 16 byte block counter */
+			BytesInBlockDiv16++;
+
+			/* Check if the current command is being aborted by the host */
+			if (MSInterfaceInfo->State.IsMassStoreReset)
+			  return;
+		}
+
+		/* Decrement the blocks remaining counter */
+		TotalBlocks--;
+	}
+
+	/* If the endpoint is full, send its contents to the host */
+	if (!(Endpoint_IsReadWriteAllowed()))
+	  Endpoint_ClearIN();
+
+	/* Deselect all Dataflash chips */
+	Dataflash_DeselectChip();
+}
+
+/** Writes blocks (OS blocks, not Dataflash pages) to the storage medium, the board Dataflash IC(s), from
+ *  the given RAM buffer. This routine reads in OS sized blocks from the buffer and writes them to the
+ *  Dataflash in Dataflash page sized blocks. This can be linked to FAT libraries to write files to the
+ *  Dataflash.
+ *
+ *  \param[in] BlockAddress  Data block starting address for the write sequence
+ *  \param[in] TotalBlocks   Number of blocks of data to write
+ *  \param[in] BufferPtr     Pointer to the data source RAM buffer
+ */
+void DataflashManager_WriteBlocks_RAM(const uint32_t BlockAddress,
+                                      uint16_t TotalBlocks,
+                                      uint8_t* BufferPtr)
+{
+	uint16_t CurrDFPage          = ((BlockAddress * VIRTUAL_MEMORY_BLOCK_SIZE) / DATAFLASH_PAGE_SIZE);
+	uint16_t CurrDFPageByte      = ((BlockAddress * VIRTUAL_MEMORY_BLOCK_SIZE) % DATAFLASH_PAGE_SIZE);
+	uint8_t  CurrDFPageByteDiv16 = (CurrDFPageByte >> 4);
+	bool     UsingSecondBuffer   = false;
+
+	/* Select the correct starting Dataflash IC for the block requested */
+	Dataflash_SelectChipFromPage(CurrDFPage);
+
+#if (DATAFLASH_PAGE_SIZE > VIRTUAL_MEMORY_BLOCK_SIZE)
+	/* Copy selected dataflash's current page contents to the Dataflash buffer */
+	Dataflash_SendByte(DF_CMD_MAINMEMTOBUFF1);
+	Dataflash_SendAddressBytes(CurrDFPage, 0);
+	Dataflash_WaitWhileBusy();
+#endif
+
+	/* Send the Dataflash buffer write command */
+	Dataflash_SendByte(DF_CMD_BUFF1WRITE);
+	Dataflash_SendAddressBytes(0, CurrDFPageByte);
+
+	while (TotalBlocks)
+	{
+		uint8_t BytesInBlockDiv16 = 0;
+
+		/* Write an endpoint packet sized data block to the Dataflash */
+		while (BytesInBlockDiv16 < (VIRTUAL_MEMORY_BLOCK_SIZE >> 4))
+		{
+			/* Check if end of Dataflash page reached */
+			if (CurrDFPageByteDiv16 == (DATAFLASH_PAGE_SIZE >> 4))
+			{
+				/* Write the Dataflash buffer contents back to the Dataflash page */
+				Dataflash_WaitWhileBusy();
+				Dataflash_SendByte(UsingSecondBuffer ? DF_CMD_BUFF2TOMAINMEMWITHERASE : DF_CMD_BUFF1TOMAINMEMWITHERASE);
+				Dataflash_SendAddressBytes(CurrDFPage, 0);
+
+				/* Reset the Dataflash buffer counter, increment the page counter */
+				CurrDFPageByteDiv16 = 0;
+				CurrDFPage++;
+
+				/* Once all the Dataflash ICs have had their first buffers filled, switch buffers to maintain throughput */
+				if (Dataflash_GetSelectedChip() == DATAFLASH_CHIP_MASK(DATAFLASH_TOTALCHIPS))
+				  UsingSecondBuffer = !(UsingSecondBuffer);
+
+				/* Select the next Dataflash chip based on the new Dataflash page index */
+				Dataflash_SelectChipFromPage(CurrDFPage);
+
+#if (DATAFLASH_PAGE_SIZE > VIRTUAL_MEMORY_BLOCK_SIZE)
+				/* If less than one Dataflash page remaining, copy over the existing page to preserve trailing data */
+				if ((TotalBlocks * (VIRTUAL_MEMORY_BLOCK_SIZE >> 4)) < (DATAFLASH_PAGE_SIZE >> 4))
+				{
+					/* Copy selected dataflash's current page contents to the Dataflash buffer */
+					Dataflash_WaitWhileBusy();
+					Dataflash_SendByte(UsingSecondBuffer ? DF_CMD_MAINMEMTOBUFF2 : DF_CMD_MAINMEMTOBUFF1);
+					Dataflash_SendAddressBytes(CurrDFPage, 0);
+					Dataflash_WaitWhileBusy();
+				}
+#endif
+
+				/* Send the Dataflash buffer write command */
+				Dataflash_ToggleSelectedChipCS();
+				Dataflash_SendByte(UsingSecondBuffer ? DF_CMD_BUFF2WRITE : DF_CMD_BUFF1WRITE);
+				Dataflash_SendAddressBytes(0, 0);
+			}
+
+			/* Write one 16-byte chunk of data to the Dataflash */
+			for (uint8_t ByteNum = 0; ByteNum < 16; ByteNum++)
+			  Dataflash_SendByte(*(BufferPtr++));
+
+			/* Increment the Dataflash page 16 byte block counter */
+			CurrDFPageByteDiv16++;
+
+			/* Increment the block 16 byte block counter */
+			BytesInBlockDiv16++;
+		}
+
+		/* Decrement the blocks remaining counter */
+		TotalBlocks--;
+	}
+
+	/* Write the Dataflash buffer contents back to the Dataflash page */
+	Dataflash_WaitWhileBusy();
+	Dataflash_SendByte(UsingSecondBuffer ? DF_CMD_BUFF2TOMAINMEMWITHERASE : DF_CMD_BUFF1TOMAINMEMWITHERASE);
+	Dataflash_SendAddressBytes(CurrDFPage, 0x00);
+	Dataflash_WaitWhileBusy();
+
+	/* Deselect all Dataflash chips */
+	Dataflash_DeselectChip();
+}
+
+/** Reads blocks (OS blocks, not Dataflash pages) from the storage medium, the board Dataflash IC(s), into
+ *  the preallocated RAM buffer. This routine reads in Dataflash page sized blocks from the Dataflash
+ *  and writes them in OS sized blocks to the given buffer. This can be linked to FAT libraries to read
+ *  the files stored on the Dataflash.
+ *
+ *  \param[in] BlockAddress  Data block starting address for the read sequence
+ *  \param[in] TotalBlocks   Number of blocks of data to read
+ *  \param[out] BufferPtr    Pointer to the data destination RAM buffer
+ */
+void DataflashManager_ReadBlocks_RAM(const uint32_t BlockAddress,
+                                     uint16_t TotalBlocks,
+                                     uint8_t* BufferPtr)
+{
+	uint16_t CurrDFPage          = ((BlockAddress * VIRTUAL_MEMORY_BLOCK_SIZE) / DATAFLASH_PAGE_SIZE);
+	uint16_t CurrDFPageByte      = ((BlockAddress * VIRTUAL_MEMORY_BLOCK_SIZE) % DATAFLASH_PAGE_SIZE);
+	uint8_t  CurrDFPageByteDiv16 = (CurrDFPageByte >> 4);
+
+	/* Select the correct starting Dataflash IC for the block requested */
+	Dataflash_SelectChipFromPage(CurrDFPage);
+
+	/* Send the Dataflash main memory page read command */
+	Dataflash_SendByte(DF_CMD_MAINMEMPAGEREAD);
+	Dataflash_SendAddressBytes(CurrDFPage, CurrDFPageByte);
+	Dataflash_SendByte(0x00);
+	Dataflash_SendByte(0x00);
+	Dataflash_SendByte(0x00);
+	Dataflash_SendByte(0x00);
+
+	while (TotalBlocks)
+	{
+		uint8_t BytesInBlockDiv16 = 0;
+
+		/* Read an endpoint packet sized data block from the Dataflash */
+		while (BytesInBlockDiv16 < (VIRTUAL_MEMORY_BLOCK_SIZE >> 4))
+		{
+			/* Check if end of Dataflash page reached */
+			if (CurrDFPageByteDiv16 == (DATAFLASH_PAGE_SIZE >> 4))
+			{
+				/* Reset the Dataflash buffer counter, increment the page counter */
+				CurrDFPageByteDiv16 = 0;
+				CurrDFPage++;
+
+				/* Select the next Dataflash chip based on the new Dataflash page index */
+				Dataflash_SelectChipFromPage(CurrDFPage);
+
+				/* Send the Dataflash main memory page read command */
+				Dataflash_SendByte(DF_CMD_MAINMEMPAGEREAD);
+				Dataflash_SendAddressBytes(CurrDFPage, 0);
+				Dataflash_SendByte(0x00);
+				Dataflash_SendByte(0x00);
+				Dataflash_SendByte(0x00);
+				Dataflash_SendByte(0x00);
+			}
+
+			/* Read one 16-byte chunk of data from the Dataflash */
+			for (uint8_t ByteNum = 0; ByteNum < 16; ByteNum++)
+			  *(BufferPtr++) = Dataflash_ReceiveByte();
+
+			/* Increment the Dataflash page 16 byte block counter */
+			CurrDFPageByteDiv16++;
+
+			/* Increment the block 16 byte block counter */
+			BytesInBlockDiv16++;
+		}
+
+		/* Decrement the blocks remaining counter */
+		TotalBlocks--;
+	}
+
+	/* Deselect all Dataflash chips */
+	Dataflash_DeselectChip();
+}
+
+/** Disables the Dataflash memory write protection bits on the board Dataflash ICs, if enabled. */
+void DataflashManager_ResetDataflashProtections(void)
+{
+	/* Select first Dataflash chip, send the read status register command */
+	Dataflash_SelectChip(DATAFLASH_CHIP1);
+	Dataflash_SendByte(DF_CMD_GETSTATUS);
+
+	/* Check if sector protection is enabled */
+	if (Dataflash_ReceiveByte() & DF_STATUS_SECTORPROTECTION_ON)
+	{
+		Dataflash_ToggleSelectedChipCS();
+
+		/* Send the commands to disable sector protection */
+		Dataflash_SendByte(DF_CMD_SECTORPROTECTIONOFF[0]);
+		Dataflash_SendByte(DF_CMD_SECTORPROTECTIONOFF[1]);
+		Dataflash_SendByte(DF_CMD_SECTORPROTECTIONOFF[2]);
+		Dataflash_SendByte(DF_CMD_SECTORPROTECTIONOFF[3]);
+	}
+
+	/* Select second Dataflash chip (if present on selected board), send read status register command */
+	#if (DATAFLASH_TOTALCHIPS == 2)
+	Dataflash_SelectChip(DATAFLASH_CHIP2);
+	Dataflash_SendByte(DF_CMD_GETSTATUS);
+
+	/* Check if sector protection is enabled */
+	if (Dataflash_ReceiveByte() & DF_STATUS_SECTORPROTECTION_ON)
+	{
+		Dataflash_ToggleSelectedChipCS();
+
+		/* Send the commands to disable sector protection */
+		Dataflash_SendByte(DF_CMD_SECTORPROTECTIONOFF[0]);
+		Dataflash_SendByte(DF_CMD_SECTORPROTECTIONOFF[1]);
+		Dataflash_SendByte(DF_CMD_SECTORPROTECTIONOFF[2]);
+		Dataflash_SendByte(DF_CMD_SECTORPROTECTIONOFF[3]);
+	}
+	#endif
+
+	/* Deselect current Dataflash chip */
+	Dataflash_DeselectChip();
+}
+
+/** Performs a simple test on the attached Dataflash IC(s) to ensure that they are working.
+ *
+ *  \return Boolean \c true if all media chips are working, \c false otherwise
+ */
+bool DataflashManager_CheckDataflashOperation(void)
+{
+	uint8_t ReturnByte;
+
+	/* Test first Dataflash IC is present and responding to commands */
+	Dataflash_SelectChip(DATAFLASH_CHIP1);
+	Dataflash_SendByte(DF_CMD_READMANUFACTURERDEVICEINFO);
+	ReturnByte = Dataflash_ReceiveByte();
+	Dataflash_DeselectChip();
+
+	/* If returned data is invalid, fail the command */
+	if (ReturnByte != DF_MANUFACTURER_ATMEL)
+	  return false;
+
+	#if (DATAFLASH_TOTALCHIPS == 2)
+	/* Test second Dataflash IC is present and responding to commands */
+	Dataflash_SelectChip(DATAFLASH_CHIP2);
+	Dataflash_SendByte(DF_CMD_READMANUFACTURERDEVICEINFO);
+	ReturnByte = Dataflash_ReceiveByte();
+	Dataflash_DeselectChip();
+
+	/* If returned data is invalid, fail the command */
+	if (ReturnByte != DF_MANUFACTURER_ATMEL)
+	  return false;
+	#endif
+
+	return true;
+}
+
diff --git a/lib/lufa/Demos/Device/ClassDriver/MassStorageKeyboard/Lib/DataflashManager.h b/lib/lufa/Demos/Device/ClassDriver/MassStorageKeyboard/Lib/DataflashManager.h
new file mode 100644
index 0000000000..076847a7d6
--- /dev/null
+++ b/lib/lufa/Demos/Device/ClassDriver/MassStorageKeyboard/Lib/DataflashManager.h
@@ -0,0 +1,87 @@
+/*
+             LUFA Library
+     Copyright (C) Dean Camera, 2017.
+
+  dean [at] fourwalledcubicle [dot] com
+           www.lufa-lib.org
+*/
+
+/*
+  Copyright 2017  Dean Camera (dean [at] fourwalledcubicle [dot] com)
+
+  Permission to use, copy, modify, distribute, and sell this
+  software and its documentation for any purpose is hereby granted
+  without fee, provided that the above copyright notice appear in
+  all copies and that both that the copyright notice and this
+  permission notice and warranty disclaimer appear in supporting
+  documentation, and that the name of the author not be used in
+  advertising or publicity pertaining to distribution of the
+  software without specific, written prior permission.
+
+  The author disclaims all warranties with regard to this
+  software, including all implied warranties of merchantability
+  and fitness.  In no event shall the author be liable for any
+  special, indirect or consequential damages or any damages
+  whatsoever resulting from loss of use, data or profits, whether
+  in an action of contract, negligence or other tortious action,
+  arising out of or in connection with the use or performance of
+  this software.
+*/
+
+/** \file
+ *
+ *  Header file for DataflashManager.c.
+ */
+
+#ifndef _DATAFLASH_MANAGER_H_
+#define _DATAFLASH_MANAGER_H_
+
+	/* Includes: */
+		#include <avr/io.h>
+
+		#include "../MassStorageKeyboard.h"
+		#include "../Descriptors.h"
+		#include "Config/AppConfig.h"
+
+		#include <LUFA/Common/Common.h>
+		#include <LUFA/Drivers/USB/USB.h>
+		#include <LUFA/Drivers/Board/Dataflash.h>
+
+	/* Preprocessor Checks: */
+		#if (DATAFLASH_PAGE_SIZE % 16)
+			#error Dataflash page size must be a multiple of 16 bytes.
+		#endif
+
+	/* Defines: */
+		/** Total number of bytes of the storage medium, comprised of one or more Dataflash ICs. */
+		#define VIRTUAL_MEMORY_BYTES                ((uint32_t)DATAFLASH_PAGES * DATAFLASH_PAGE_SIZE * DATAFLASH_TOTALCHIPS)
+
+		/** Block size of the device. This is kept at 512 to remain compatible with the OS despite the underlying
+		 *  storage media (Dataflash) using a different native block size.
+		 */
+		#define VIRTUAL_MEMORY_BLOCK_SIZE           512
+
+		/** Total number of blocks of the virtual memory for reporting to the host as the device's total capacity. */
+		#define VIRTUAL_MEMORY_BLOCKS              (VIRTUAL_MEMORY_BYTES / VIRTUAL_MEMORY_BLOCK_SIZE)
+
+		/** Blocks in each LUN, calculated from the total capacity divided by the total number of Logical Units in the device. */
+		#define LUN_MEDIA_BLOCKS         (VIRTUAL_MEMORY_BLOCKS / TOTAL_LUNS)
+
+	/* Function Prototypes: */
+		void DataflashManager_WriteBlocks(USB_ClassInfo_MS_Device_t* const MSInterfaceInfo,
+		                                  const uint32_t BlockAddress,
+		                                  uint16_t TotalBlocks);
+		void DataflashManager_ReadBlocks(USB_ClassInfo_MS_Device_t* const MSInterfaceInfo,
+		                                 const uint32_t BlockAddress,
+		                                 uint16_t TotalBlocks);
+		void DataflashManager_WriteBlocks_RAM(const uint32_t BlockAddress,
+		                                      uint16_t TotalBlocks,
+		                                      uint8_t* BufferPtr) ATTR_NON_NULL_PTR_ARG(3);
+		void DataflashManager_ReadBlocks_RAM(const uint32_t BlockAddress,
+		                                     uint16_t TotalBlocks,
+		                                     uint8_t* BufferPtr) ATTR_NON_NULL_PTR_ARG(3);
+		void DataflashManager_ResetDataflashProtections(void);
+		bool DataflashManager_CheckDataflashOperation(void);
+
+#endif
+
diff --git a/lib/lufa/Demos/Device/ClassDriver/MassStorageKeyboard/Lib/SCSI.c b/lib/lufa/Demos/Device/ClassDriver/MassStorageKeyboard/Lib/SCSI.c
new file mode 100644
index 0000000000..8780d1603d
--- /dev/null
+++ b/lib/lufa/Demos/Device/ClassDriver/MassStorageKeyboard/Lib/SCSI.c
@@ -0,0 +1,349 @@
+/*
+             LUFA Library
+     Copyright (C) Dean Camera, 2017.
+
+  dean [at] fourwalledcubicle [dot] com
+           www.lufa-lib.org
+*/
+
+/*
+  Copyright 2017  Dean Camera (dean [at] fourwalledcubicle [dot] com)
+
+  Permission to use, copy, modify, distribute, and sell this
+  software and its documentation for any purpose is hereby granted
+  without fee, provided that the above copyright notice appear in
+  all copies and that both that the copyright notice and this
+  permission notice and warranty disclaimer appear in supporting
+  documentation, and that the name of the author not be used in
+  advertising or publicity pertaining to distribution of the
+  software without specific, written prior permission.
+
+  The author disclaims all warranties with regard to this
+  software, including all implied warranties of merchantability
+  and fitness.  In no event shall the author be liable for any
+  special, indirect or consequential damages or any damages
+  whatsoever resulting from loss of use, data or profits, whether
+  in an action of contract, negligence or other tortious action,
+  arising out of or in connection with the use or performance of
+  this software.
+*/
+
+/** \file
+ *
+ *  SCSI command processing routines, for SCSI commands issued by the host. Mass Storage
+ *  devices use a thin "Bulk-Only Transport" protocol for issuing commands and status information,
+ *  which wrap around standard SCSI device commands for controlling the actual storage medium.
+ */
+
+#define  INCLUDE_FROM_SCSI_C
+#include "SCSI.h"
+
+/** Structure to hold the SCSI response data to a SCSI INQUIRY command. This gives information about the device's
+ *  features and capabilities.
+ */
+static const SCSI_Inquiry_Response_t InquiryData =
+	{
+		.DeviceType          = DEVICE_TYPE_BLOCK,
+		.PeripheralQualifier = 0,
+
+		.Removable           = true,
+
+		.Version             = 0,
+
+		.ResponseDataFormat  = 2,
+		.NormACA             = false,
+		.TrmTsk              = false,
+		.AERC                = false,
+
+		.AdditionalLength    = 0x1F,
+
+		.SoftReset           = false,
+		.CmdQue              = false,
+		.Linked              = false,
+		.Sync                = false,
+		.WideBus16Bit        = false,
+		.WideBus32Bit        = false,
+		.RelAddr             = false,
+
+		.VendorID            = "LUFA",
+		.ProductID           = "Dataflash Disk",
+		.RevisionID          = {'0','.','0','0'},
+	};
+
+/** Structure to hold the sense data for the last issued SCSI command, which is returned to the host after a SCSI REQUEST SENSE
+ *  command is issued. This gives information on exactly why the last command failed to complete.
+ */
+static SCSI_Request_Sense_Response_t SenseData =
+	{
+		.ResponseCode        = 0x70,
+		.AdditionalLength    = 0x0A,
+	};
+
+
+/** Main routine to process the SCSI command located in the Command Block Wrapper read from the host. This dispatches
+ *  to the appropriate SCSI command handling routine if the issued command is supported by the device, else it returns
+ *  a command failure due to a ILLEGAL REQUEST.
+ *
+ *  \param[in] MSInterfaceInfo  Pointer to the Mass Storage class interface structure that the command is associated with
+ *
+ *  \return Boolean \c true if the command completed successfully, \c false otherwise
+ */
+bool SCSI_DecodeSCSICommand(USB_ClassInfo_MS_Device_t* const MSInterfaceInfo)
+{
+	bool CommandSuccess = false;
+
+	/* Run the appropriate SCSI command hander function based on the passed command */
+	switch (MSInterfaceInfo->State.CommandBlock.SCSICommandData[0])
+	{
+		case SCSI_CMD_INQUIRY:
+			CommandSuccess = SCSI_Command_Inquiry(MSInterfaceInfo);
+			break;
+		case SCSI_CMD_REQUEST_SENSE:
+			CommandSuccess = SCSI_Command_Request_Sense(MSInterfaceInfo);
+			break;
+		case SCSI_CMD_READ_CAPACITY_10:
+			CommandSuccess = SCSI_Command_Read_Capacity_10(MSInterfaceInfo);
+			break;
+		case SCSI_CMD_SEND_DIAGNOSTIC:
+			CommandSuccess = SCSI_Command_Send_Diagnostic(MSInterfaceInfo);
+			break;
+		case SCSI_CMD_WRITE_10:
+			CommandSuccess = SCSI_Command_ReadWrite_10(MSInterfaceInfo, DATA_WRITE);
+			break;
+		case SCSI_CMD_READ_10:
+			CommandSuccess = SCSI_Command_ReadWrite_10(MSInterfaceInfo, DATA_READ);
+			break;
+		case SCSI_CMD_MODE_SENSE_6:
+			CommandSuccess = SCSI_Command_ModeSense_6(MSInterfaceInfo);
+			break;
+		case SCSI_CMD_START_STOP_UNIT:
+		case SCSI_CMD_TEST_UNIT_READY:
+		case SCSI_CMD_PREVENT_ALLOW_MEDIUM_REMOVAL:
+		case SCSI_CMD_VERIFY_10:
+			/* These commands should just succeed, no handling required */
+			CommandSuccess = true;
+			MSInterfaceInfo->State.CommandBlock.DataTransferLength = 0;
+			break;
+		default:
+			/* Update the SENSE key to reflect the invalid command */
+			SCSI_SET_SENSE(SCSI_SENSE_KEY_ILLEGAL_REQUEST,
+		                   SCSI_ASENSE_INVALID_COMMAND,
+		                   SCSI_ASENSEQ_NO_QUALIFIER);
+			break;
+	}
+
+	/* Check if command was successfully processed */
+	if (CommandSuccess)
+	{
+		SCSI_SET_SENSE(SCSI_SENSE_KEY_GOOD,
+		               SCSI_ASENSE_NO_ADDITIONAL_INFORMATION,
+		               SCSI_ASENSEQ_NO_QUALIFIER);
+
+		return true;
+	}
+
+	return false;
+}
+
+/** Command processing for an issued SCSI INQUIRY command. This command returns information about the device's features
+ *  and capabilities to the host.
+ *
+ *  \param[in] MSInterfaceInfo  Pointer to the Mass Storage class interface structure that the command is associated with
+ *
+ *  \return Boolean \c true if the command completed successfully, \c false otherwise.
+ */
+static bool SCSI_Command_Inquiry(USB_ClassInfo_MS_Device_t* const MSInterfaceInfo)
+{
+	uint16_t AllocationLength  = SwapEndian_16(*(uint16_t*)&MSInterfaceInfo->State.CommandBlock.SCSICommandData[3]);
+	uint16_t BytesTransferred  = MIN(AllocationLength, sizeof(InquiryData));
+
+	/* Only the standard INQUIRY data is supported, check if any optional INQUIRY bits set */
+	if ((MSInterfaceInfo->State.CommandBlock.SCSICommandData[1] & ((1 << 0) | (1 << 1))) ||
+	     MSInterfaceInfo->State.CommandBlock.SCSICommandData[2])
+	{
+		/* Optional but unsupported bits set - update the SENSE key and fail the request */
+		SCSI_SET_SENSE(SCSI_SENSE_KEY_ILLEGAL_REQUEST,
+		               SCSI_ASENSE_INVALID_FIELD_IN_CDB,
+		               SCSI_ASENSEQ_NO_QUALIFIER);
+
+		return false;
+	}
+
+	Endpoint_Write_Stream_LE(&InquiryData, BytesTransferred, NULL);
+
+	/* Pad out remaining bytes with 0x00 */
+	Endpoint_Null_Stream((AllocationLength - BytesTransferred), NULL);
+
+	/* Finalize the stream transfer to send the last packet */
+	Endpoint_ClearIN();
+
+	/* Succeed the command and update the bytes transferred counter */
+	MSInterfaceInfo->State.CommandBlock.DataTransferLength -= BytesTransferred;
+
+	return true;
+}
+
+/** Command processing for an issued SCSI REQUEST SENSE command. This command returns information about the last issued command,
+ *  including the error code and additional error information so that the host can determine why a command failed to complete.
+ *
+ *  \param[in] MSInterfaceInfo  Pointer to the Mass Storage class interface structure that the command is associated with
+ *
+ *  \return Boolean \c true if the command completed successfully, \c false otherwise.
+ */
+static bool SCSI_Command_Request_Sense(USB_ClassInfo_MS_Device_t* const MSInterfaceInfo)
+{
+	uint8_t  AllocationLength = MSInterfaceInfo->State.CommandBlock.SCSICommandData[4];
+	uint8_t  BytesTransferred = MIN(AllocationLength, sizeof(SenseData));
+
+	Endpoint_Write_Stream_LE(&SenseData, BytesTransferred, NULL);
+	Endpoint_Null_Stream((AllocationLength - BytesTransferred), NULL);
+	Endpoint_ClearIN();
+
+	/* Succeed the command and update the bytes transferred counter */
+	MSInterfaceInfo->State.CommandBlock.DataTransferLength -= BytesTransferred;
+
+	return true;
+}
+
+/** Command processing for an issued SCSI READ CAPACITY (10) command. This command returns information about the device's capacity
+ *  on the selected Logical Unit (drive), as a number of OS-sized blocks.
+ *
+ *  \param[in] MSInterfaceInfo  Pointer to the Mass Storage class interface structure that the command is associated with
+ *
+ *  \return Boolean \c true if the command completed successfully, \c false otherwise.
+ */
+static bool SCSI_Command_Read_Capacity_10(USB_ClassInfo_MS_Device_t* const MSInterfaceInfo)
+{
+	uint32_t LastBlockAddressInLUN = (LUN_MEDIA_BLOCKS - 1);
+	uint32_t MediaBlockSize        = VIRTUAL_MEMORY_BLOCK_SIZE;
+
+	Endpoint_Write_Stream_BE(&LastBlockAddressInLUN, sizeof(LastBlockAddressInLUN), NULL);
+	Endpoint_Write_Stream_BE(&MediaBlockSize, sizeof(MediaBlockSize), NULL);
+	Endpoint_ClearIN();
+
+	/* Succeed the command and update the bytes transferred counter */
+	MSInterfaceInfo->State.CommandBlock.DataTransferLength -= 8;
+
+	return true;
+}
+
+/** Command processing for an issued SCSI SEND DIAGNOSTIC command. This command performs a quick check of the Dataflash ICs on the
+ *  board, and indicates if they are present and functioning correctly. Only the Self-Test portion of the diagnostic command is
+ *  supported.
+ *
+ *  \param[in] MSInterfaceInfo  Pointer to the Mass Storage class interface structure that the command is associated with
+ *
+ *  \return Boolean \c true if the command completed successfully, \c false otherwise.
+ */
+static bool SCSI_Command_Send_Diagnostic(USB_ClassInfo_MS_Device_t* const MSInterfaceInfo)
+{
+	/* Check to see if the SELF TEST bit is not set */
+	if (!(MSInterfaceInfo->State.CommandBlock.SCSICommandData[1] & (1 << 2)))
+	{
+		/* Only self-test supported - update SENSE key and fail the command */
+		SCSI_SET_SENSE(SCSI_SENSE_KEY_ILLEGAL_REQUEST,
+		               SCSI_ASENSE_INVALID_FIELD_IN_CDB,
+		               SCSI_ASENSEQ_NO_QUALIFIER);
+
+		return false;
+	}
+
+	/* Check to see if all attached Dataflash ICs are functional */
+	if (!(DataflashManager_CheckDataflashOperation()))
+	{
+		/* Update SENSE key with a hardware error condition and return command fail */
+		SCSI_SET_SENSE(SCSI_SENSE_KEY_HARDWARE_ERROR,
+		               SCSI_ASENSE_NO_ADDITIONAL_INFORMATION,
+		               SCSI_ASENSEQ_NO_QUALIFIER);
+
+		return false;
+	}
+
+	/* Succeed the command and update the bytes transferred counter */
+	MSInterfaceInfo->State.CommandBlock.DataTransferLength = 0;
+
+	return true;
+}
+
+/** Command processing for an issued SCSI READ (10) or WRITE (10) command. This command reads in the block start address