- add support for flashing to DFU

git-svn-id: https://svn.openpcd.org:2342/trunk@210 6dc7ffe9-61d6-0310-9af1-9938baff3ed1

1 files changed, 28 insertions, 14 deletions

diff --git a/firmware/src/dfu/flash.c b/firmware/src/dfu/flash.c
index 675552d..8806bae 100644
--- a/firmware/src/dfu/flash.c
+++ b/firmware/src/dfu/flash.c
@@ -1,4 +1,6 @@
-
+#include <sys/types.h>
+#include <lib_AT91SAM7.h>
+#include <AT91SAM7.h>
 
 #define EFCS_CMD_WRITE_PAGE		0x01
 #define EFCS_CMD_SET_LOCK_BIT		0x02
@@ -10,26 +12,38 @@
 #define EFCS_CMD_SET_SECURITY_BIT	0x0f
 
 
-int unlock_page(u_int16_t page)
+static u_int16_t page_from_ramaddr(const void *addr)
 {
-	AT91C_MC_FCMD_UNLOCK | AT91C_MC_CORRECT_KEY | 
-
+	u_int32_t ramaddr = (u_int32_t) addr;
+	ramaddr -= (u_int32_t) AT91C_IFLASH;
+	return (ramaddr >> AT91C_IFLASH_PAGE_SHIFT);
 }
+#define PAGES_PER_LOCKREGION	(AT91C_IFLASH_LOCK_REGION_SIZE>>AT91C_IFLASH_PAGE_SHIFT)
+#define IS_FIRST_PAGE_OF_LOCKREGION(x)	((x % PAGES_PER_LOCKREGION) == 0)
+#define LOCKREGION_FROM_PAGE(x)	(x / PAGES_PER_LOCKREGION)
 
-int flash_sector(unsigned int sector, const u_int8_t *data, unsigned int len)
+static int is_page_locked(u_int16_t page)
 {
-	volatile u_int32_t *p = (volatile u_int32_t *)0;
-	u_int32_t *src32 = (u_int32_t *)data;
-	int i;
+	u_int16_t lockregion = LOCKREGION_FROM_PAGE(page);
 
-	/* hand-code memcpy because we need to make sure only 32bit accesses
-	 * are used */
-	for (i = 0; i < len/4; i++)
-		p[i] = src32[i];
+	return (AT91C_BASE_MC->MC_FSR & (lockregion << 16));
+}
 
-	AT91F_MC_EFC_PerformCmd(pmc , AT91C_MC_FCMD_START_PROG|
-				AT91C_MC_CORRECT_KEY | );
+static void unlock_page(u_int16_t page)
+{
+	AT91F_MC_EFC_PerformCmd(AT91C_BASE_MC, AT91C_MC_FCMD_UNLOCK |
+				AT91C_MC_CORRECT_KEY | page);
 }
 
+void flash_page(u_int8_t *addr)
+{
+	u_int16_t page = page_from_ramaddr(addr);
+
+	if (is_page_locked(page))
+		unlock_page(page);
+
+	AT91F_MC_EFC_PerformCmd(AT91C_BASE_MC, AT91C_MC_FCMD_START_PROG |
+				AT91C_MC_CORRECT_KEY | page);
+}