X-Git-Url: https://codewiz.org/gitweb?a=blobdiff_plain;f=bertos%2Fcpu%2Farm%2Fdrv%2Fflash_at91.c;h=50d6b781116c08c9c051c7d7abd0c97924273517;hb=705e92ca3e6a5bd0223f21a8fd9ac714f3253a5f;hp=514d54e4326811042b4ea7dcafcf0e1f5e815857;hpb=540b160dbedfdb12ce29f9b107be860ba22cd6cb;p=bertos.git

diff --git a/bertos/cpu/arm/drv/flash_at91.c b/bertos/cpu/arm/drv/flash_at91.c
index 514d54e4..50d6b781 100644
--- a/bertos/cpu/arm/drv/flash_at91.c
+++ b/bertos/cpu/arm/drv/flash_at91.c
@@ -75,11 +75,27 @@ struct FlashHardware
  */
 RAM_FUNC NOINLINE static void write_page(uint32_t page)
 {
+	reg32_t *fcr, *fsr;
+
+	#if CPU_ARM_AT91SAM7S512 || CPU_ARM_AT91SAM7X512
+	if (page >= (FLASH_MEM_SIZE / FLASH_PAGE_SIZE_BYTES / 2))
+	{
+		fcr = &MC_FCR1;
+		fsr = &MC_FSR1;
+		page &= 0x3FF;
+	}
+	else
+	#endif
+	{
+		fcr = &MC_FCR;
+		fsr = &MC_FSR;
+	}
+
 	// Send the 'write page' command
-	MC_FCR = MC_KEY | MC_FCMD_WP | (MC_PAGEN_MASK & (page << 8));
+	*fcr = MC_KEY | MC_FCMD_WP | (MC_PAGEN_MASK & (page << 8));
 
 	// Wait for the end of command
-	while(!(MC_FSR & BV(MC_FRDY)))
+	while(!(*fsr & BV(MC_FRDY)))
 	{
 		//NOP;
 	}
@@ -133,6 +149,23 @@ static bool flash_getStatus(struct KBlock *blk)
 		LOG_ERR("wr protect..\n");
 		return false;
 	}
+	#if CPU_ARM_AT91SAM7X512 || CPU_ARM_AT91SAM7S512
+		/* Check also EFC1 for larger devices */
+		if(MC_FSR1 & BV(MC_PROGE))
+		{
+			fls->hw->status |= FLASH_WR_ERR;
+			LOG_ERR("flash not erased..\n");
+			return false;
+		}
+
+		if(MC_FSR1 & BV(MC_LOCKE))
+		{
+			fls->hw->status |= FLASH_WR_PROTECT;
+			LOG_ERR("wr protect..\n");
+			return false;
+		}
+
+	#endif
 
 	return true;
 }