X-Git-Url: http://git.linex4red.de/pub/lufa.git/blobdiff_plain/0f0f9638a30d4267db80f235a7f3e4883a1d9f9d..726b325c73d2f6a2a733e67d1dad9fa7aedef1c0:/Bootloaders/Incomplete/MassStorage/Lib/VirtualFAT.c

diff --git a/Bootloaders/Incomplete/MassStorage/Lib/VirtualFAT.c b/Bootloaders/Incomplete/MassStorage/Lib/VirtualFAT.c
index d43ac86cb..5d91cefaf 100644
--- a/Bootloaders/Incomplete/MassStorage/Lib/VirtualFAT.c
+++ b/Bootloaders/Incomplete/MassStorage/Lib/VirtualFAT.c
@@ -92,7 +92,7 @@ static void UpdateFAT12ClusterEntry(uint8_t* FATTable,
 
 static void WriteBlock(const uint16_t BlockNumber)
 {
-	uint8_t BlockBuffer[512];
+	uint8_t BlockBuffer[SECTOR_SIZE_BYTES];
 
 	/* Wait until endpoint is ready before continuing */
 	if (Endpoint_WaitUntilReady())
@@ -101,13 +101,40 @@ static void WriteBlock(const uint16_t BlockNumber)
 	Endpoint_Read_Stream_LE(BlockBuffer, sizeof(BlockBuffer), NULL);
 	Endpoint_ClearOUT();
 
-	printf("WRITE %d\r\n", BlockNumber);
-	// TODO: Write to FLASH
+	if ((BlockNumber >= 4) && (BlockNumber < (4 + (FIRMWARE_FILE_SIZE / SECTOR_SIZE_BYTES))))
+	{
+		uint32_t WriteFlashAddress = (uint32_t)(BlockNumber - 4) * SECTOR_SIZE_BYTES;
+
+		for (uint16_t i = 0; i < SECTOR_SIZE_BYTES; i += 2)
+		{
+			/* Disallow writing to the bootloader section */
+			if (WriteFlashAddress > BOOT_START_ADDR)
+			  continue;
+
+			if ((WriteFlashAddress % SPM_PAGESIZE) == 0)
+			{
+				/* Erase the given FLASH page, ready to be programmed */
+				boot_page_erase(WriteFlashAddress);
+				boot_spm_busy_wait();
+			}
+
+			/* Write the next data word to the FLASH page */
+			boot_page_fill(WriteFlashAddress, (BlockBuffer[i + 1] << 8) | BlockBuffer[i]);
+			WriteFlashAddress += 2;
+
+			if ((WriteFlashAddress % SPM_PAGESIZE) == 0)
+			{
+				/* Write the filled FLASH page to memory */
+				boot_page_write(WriteFlashAddress - SPM_PAGESIZE);
+				boot_spm_busy_wait();
+			}
+		}
+	}
 }
 
 static void ReadBlock(const uint16_t BlockNumber)
 {
-	uint8_t BlockBuffer[512];
+	uint8_t BlockBuffer[SECTOR_SIZE_BYTES];
 	memset(BlockBuffer, 0x00, sizeof(BlockBuffer));
 
 	switch (BlockNumber)
@@ -139,14 +166,10 @@ static void ReadBlock(const uint16_t BlockNumber)
 		default:
 			if ((BlockNumber >= 4) && (BlockNumber < (4 + (FIRMWARE_FILE_SIZE / SECTOR_SIZE_BYTES))))
 			{
-//				printf("<D>\r\n");
+				uint32_t ReadFlashAddress = (uint32_t)(BlockNumber - 4) * SECTOR_SIZE_BYTES;
 
-				for (uint16_t i = 0; i < 512; i++)
-				  BlockBuffer[i] = 'A' + (i % 26);
-			}
-			else
-			{
-				printf("INVALID %d\r\n", BlockNumber);
+				for (uint16_t i = 0; i < SECTOR_SIZE_BYTES; i++)
+				  BlockBuffer[i] = pgm_read_byte_far(ReadFlashAddress++);
 			}
 
 			break;