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

diff --git a/bertos/cpu/arm/drv/flash_at91.c b/bertos/cpu/arm/drv/flash_at91.c
index d501102a..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,13 +149,30 @@ 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;
 }
 
 static size_t at91_flash_readDirect(struct KBlock *blk, block_idx_t idx, void *buf, size_t offset, size_t size)
 {
-	memcpy(buf, (void *)(idx * blk->blk_size +  FLASH_BASE), size);
+	memcpy(buf, (void *)(idx * blk->blk_size +  FLASH_BASE + offset), size);
 	return size;
 }
 
@@ -222,21 +255,20 @@ static void common_init(Flash *fls)
 	fls->blk.blk_cnt = FLASH_MEM_SIZE / FLASH_PAGE_SIZE_BYTES;
 }
 
-void flash_hw_init(Flash *fls, int flags)
+void flash_hw_init(Flash *fls, UNUSED_ARG(int, flags))
 {
 	common_init(fls);
 	fls->blk.priv.vt = &flash_at91_buffered_vt;
-	fls->blk.priv.flags |= KB_BUFFERED | KB_PARTIAL_WRITE | flags;
+	fls->blk.priv.flags |= KB_BUFFERED | KB_PARTIAL_WRITE;
 	fls->blk.priv.buf = flash_buf;
 
 	/* Load the first block in the cache */
 	memcpy(fls->blk.priv.buf, (void *)(FLASH_BASE), fls->blk.blk_size);
 }
 
-void flash_hw_initUnbuffered(Flash *fls, int flags)
+void flash_hw_initUnbuffered(Flash *fls, UNUSED_ARG(int, flags))
 {
 	common_init(fls);
 	fls->blk.priv.vt = &flash_at91_unbuffered_vt;
-	fls->blk.priv.flags |= flags;
 }