Refactor to use new protocol module and sipo.

[bertos.git] / bertos / cpu / arm / drv / flash_at91.c

diff --git a/bertos/cpu/arm/drv/flash_at91.c b/bertos/cpu/arm/drv/flash_at91.c
index d501102a63d413b77bd3e014d173f11584a28d8e..50d6b781116c08c9c051c7d7abd0c97924273517 100644 (file)
--- 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;
 }