DFU works (fix various bugs such as forgetting to shift the page number, checking for invalid page numbers, off-by-one error in flash page calculation, etc.)

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

7 files changed, 176 insertions, 93 deletions

diff --git a/firmware/Makefile.dfu b/firmware/Makefile.dfu
index 10c3616..47edebb 100644
--- a/firmware/Makefile.dfu
+++ b/firmware/Makefile.dfu
@@ -150,26 +150,30 @@ CDEFS =  -D$(RUN_MODE) -D__MS_types__ -D__LIBRFID__
 
 ifdef DEBUG
 CDEFS += -DDEBUG 
+ADEFS += -DDEBUG 
 endif
 
-ifdef OLIMEX
+ifeq ($(BOARD),OLIMEX)
 CDEFS += -DOLIMEX
+ADEFS += -DOLIMEX
 endif
 
 ifeq ($(BOARD),PICC)
 CDEFS += -DPICC
+ADEFS += -DPICC
 endif
 
 ifeq ($(BOARD),PCD)
 SUBMDL = AT91SAM7S128
 CDEFS += -DPCD
+ADEFS += -DPCD
 endif
 
 # Place -I options here
 CINCS = -Iinclude -Isrc
 
 # Place -D or -U options for ASM here
-ADEFS =  -D$(RUN_MODE)
+ADEFS +=  -D$(RUN_MODE)
 
 ifdef VECTOR_LOCATION
 CDEFS += -D$(VECTOR_LOCATION)
diff --git a/firmware/link/AT91SAM7S256-ROM-sam7dfu-dfu.ld b/firmware/link/AT91SAM7S256-ROM-sam7dfu-dfu.ld
index 7f133da..ca450a3 100644
--- a/firmware/link/AT91SAM7S256-ROM-sam7dfu-dfu.ld
+++ b/firmware/link/AT91SAM7S256-ROM-sam7dfu-dfu.ld
@@ -45,9 +45,9 @@ SECTIONS
   	_edata = . ;
 	PROVIDE (edata = .);
 	
-	/* dfu.functab addresses are valid as long as DFU is in flash, i.e. only while
-	 * application calls those functions.  Once executing in DFU mode, we are
-	 * re-located to ram */
+	/* dfu.functab addresses are valid as long as DFU is in flash, i.e.
+	 * only while application calls those functions.  Once executing in
+	 * DFU mode, we are re-located to ram */
 	.dfu.functab 0x00010fd0: AT ( ADDR(.dfu.functab) - ADDR(.text) ) {
   		*(.dfu.functab)
   		. = ALIGN(4096);
diff --git a/firmware/src/dfu/dbgu.c b/firmware/src/dfu/dbgu.c
index 725d495..4a8d68f 100644
--- a/firmware/src/dfu/dbgu.c
+++ b/firmware/src/dfu/dbgu.c
@@ -48,16 +48,17 @@ void AT91F_DBGU_Init(void)
 	/* Open PIO for DBGU */
 	AT91F_DBGU_CfgPIO();
 	/* Enable Transmitter & receivier */
-	((AT91PS_USART) AT91C_BASE_DBGU)->US_CR = AT91C_US_RSTTX | AT91C_US_RSTRX;
+	((AT91PS_USART) AT91C_BASE_DBGU)->US_CR = 
+					AT91C_US_RSTTX | AT91C_US_RSTRX;
 
 	/* Configure DBGU */
-	AT91F_US_Configure((AT91PS_USART) AT91C_BASE_DBGU,	// DBGU base address
-			   MCK, AT91C_US_ASYNC_MODE,	// Mode Register to be programmed
-			   AT91C_DBGU_BAUD,	// Baudrate to be programmed
-			   0);	// Timeguard to be programmed
+	AT91F_US_Configure(AT91C_BASE_DBGU,
+			   MCK, AT91C_US_ASYNC_MODE,
+			   AT91C_DBGU_BAUD, 0);
 
 	/* Enable Transmitter & receivier */
-	((AT91PS_USART) AT91C_BASE_DBGU)->US_CR = AT91C_US_RXEN | AT91C_US_TXEN;
+	((AT91PS_USART) AT91C_BASE_DBGU)->US_CR = 
+					AT91C_US_RXEN | AT91C_US_TXEN;
 
 	/* Enable USART IT error and AT91C_US_ENDRX */
 	AT91F_US_EnableIt((AT91PS_USART) AT91C_BASE_DBGU, AT91C_US_RXRDY);
diff --git a/firmware/src/dfu/dfu.c b/firmware/src/dfu/dfu.c
index 4e39f59..bb8d057 100644
--- a/firmware/src/dfu/dfu.c
+++ b/firmware/src/dfu/dfu.c
@@ -36,19 +36,26 @@
 
 /* If debug is enabled, we need to access debug functions from flash
  * and therefore have to omit flashing */
-#define DEBUG_DFU
+//#define DEBUG_DFU_NOFLASH
 
-#define DEBUGE DEBUGP
+#define DEBUG_DFU_EP0
+//#define DEBUG_DFU_RECV
 
-static void flash_init(void)
-{
-	unsigned int fmcn = AT91F_MC_EFC_ComputeFMCN(MCK);
+#ifdef DEBUG_DFU_EP0
+#define DEBUGE DEBUGP
+#else
+#define DEBUGE(x, args ...)
+#endif
 
-	AT91F_MC_EFC_CfgModeReg(AT91C_BASE_MC, fmcn << 16 | AT91C_MC_FWS_3FWS |
-				AT91C_MC_FRDY | AT91C_MC_LOCKE | 
-				AT91C_MC_PROGE);
+#ifdef DEBUG_DFU_RECV
+#define DEBUGR DEBUGP
+#else
+#define DEBUGR(x, args ...)
+#endif
 
-}
+#define RET_NOTHING	0
+#define RET_ZLP		1
+#define RET_STALL	2
 
 static void __dfufunc udp_init(void)
 {
@@ -105,8 +112,22 @@ static void __dfufunc udp_ep0_send_data(const char *pData, u_int32_t length)
 	}
 }
 
+static void udp_ep0_recv_clean(void)
+{
+	unsigned int i;
+	u_int8_t dummy;
+	const AT91PS_UDP pUdp = AT91C_BASE_UDP;
+
+	while (!(pUdp->UDP_CSR[0] & AT91C_UDP_RX_DATA_BK0)) ;
+
+	for (i = 0; i < (pUdp->UDP_CSR[0] >> 16); i++)
+		dummy = pUdp->UDP_FDR[0];
+
+	pUdp->UDP_CSR[0] &= ~(AT91C_UDP_RX_DATA_BK0);
+}
+
 /* receive data from EP0 */
-static int __dfufunc udp_ep0_recv_data(char *data, u_int32_t len)
+static int __dfufunc udp_ep0_recv_data(u_int8_t *data, u_int16_t len)
 {
 	AT91PS_UDP pUdp = AT91C_BASE_UDP;
 	AT91_REG csr;
@@ -118,7 +139,7 @@ static int __dfufunc udp_ep0_recv_data(char *data, u_int32_t len)
 		 * by a RX SETUP stage? */
 		do {
 			csr = pUdp->UDP_CSR[0];
-			DEBUGE("CSR=%08x ", csr);
+			DEBUGR("CSR=%08x ", csr);
 		} while (!(csr & AT91C_UDP_RX_DATA_BK0)) ;
 	
 		num_rcv = pUdp->UDP_CSR[0] >> 16;
@@ -127,7 +148,7 @@ static int __dfufunc udp_ep0_recv_data(char *data, u_int32_t len)
 		if (num_rcv_total + num_rcv > len)
 			num_rcv = num_rcv_total - len;
 
-		DEBUGE("num_rcv = %u ", num_rcv);
+		DEBUGR("num_rcv = %u ", num_rcv);
 		for (i = 0; i < num_rcv; i++)
 			*data++ = pUdp->UDP_FDR[0];
 		pUdp->UDP_CSR[0] &= ~(AT91C_UDP_RX_DATA_BK0);
@@ -167,64 +188,72 @@ static void __dfufunc udp_ep0_send_stall(void)
 static u_int8_t status;
 static u_int8_t *ptr = (u_int8_t *) AT91C_IFLASH + SAM7DFU_SIZE;
 static __dfudata u_int32_t dfu_state = DFU_STATE_appIDLE;
-static u_int8_t pagebuf[AT91C_IFLASH_PAGE_SIZE];
+static u_int32_t pagebuf32[AT91C_IFLASH_PAGE_SIZE/4];
 
 static int __dfufunc handle_dnload(u_int16_t val, u_int16_t len)
 {
 	volatile u_int32_t *p = (volatile u_int32_t *)ptr;
+	u_int8_t *pagebuf = (u_int8_t *) pagebuf32;
+	int i;
 
 	DEBUGE("download ");
 
 	if (len > AT91C_IFLASH_PAGE_SIZE) {
 		/* Too big. Not that we'd really care, but it's a
 		 * DFU protocol violation */
+		DEBUGP("length exceeds flash page size ");
 	    	dfu_state = DFU_STATE_dfuERROR;
 		status = DFU_STATUS_errADDRESS;
-		udp_ep0_send_stall();
-		return -EINVAL;
+		return RET_STALL;
 	}
 	if (len & 0x3) {
 		/* reject non-four-byte-aligned writes */
+		DEBUGP("not four-byte-aligned length ");
 		dfu_state = DFU_STATE_dfuERROR;
 		status = DFU_STATUS_errADDRESS;
-		udp_ep0_send_stall();
-		return -EINVAL;
+		return RET_STALL;
 	}
 	if (len == 0) {
+		DEBUGP("zero-size write -> MANIFEST_SYNC ");
+		flash_page(p);
 		dfu_state = DFU_STATE_dfuMANIFEST_SYNC;
-		return 0;
+		return RET_ZLP;
 	}
 	if (ptr + len > (u_int8_t *) AT91C_IFLASH + AT91C_IFLASH_SIZE) {
+		DEBUGP("end of write exceeds flash end ");
 		dfu_state = DFU_STATE_dfuERROR;
 		status = DFU_STATUS_errADDRESS;
-		udp_ep0_send_stall();
-		return -EINVAL;
+		return RET_STALL;
 	}
 
-	udp_ep0_recv_data(pagebuf, sizeof(pagebuf));
+	DEBUGP("try_to_recv=%u ", len);
+	udp_ep0_recv_data(pagebuf, len);
 
-	DEBUGP(hexdump(pagebuf, len));
+	DEBUGR(hexdump(pagebuf, len));
 
 	/* we can only access the write buffer with correctly aligned
 	 * 32bit writes ! */
-#ifndef DEBUG_DFU
+#ifndef DEBUG_DFU_NOFLASH
+	DEBUGP("copying ");
 	for (i = 0; i < len/4; i++) {
-		*p++ = (u_int32_t *) (pagebuf[i*4]);
+		*p++ = pagebuf32[i];
 		/* If we have filled a page buffer, flash it */
-		if ((p % AT91C_FLASH_PAGE_SIZE) == 0)
-			flash_page(p-4);
+		if (((unsigned long)p % AT91C_IFLASH_PAGE_SIZE) == 0) {
+			DEBUGP("page_full  ");
+			flash_page(p-1);
+		}
 	}
-	ptr = p;
+	ptr = (u_int8_t *) p;
 #endif
 
-	return 0;
+	return RET_ZLP;
 }
 
+#define AT91C_IFLASH_END ((u_int8_t *)AT91C_IFLASH + AT91C_IFLASH_SIZE)
 static __dfufunc int handle_upload(u_int16_t val, u_int16_t len)
 {
 	DEBUGE("upload ");
-	if (len > AT91C_IFLASH_PAGE_SIZE
-	    || ptr > (u_int8_t *) AT91C_IFLASH + AT91C_IFLASH_SIZE - SAM7DFU_SIZE) {
+	if (len > AT91C_IFLASH_PAGE_SIZE) {
 		/* Too big */
 		dfu_state = DFU_STATE_dfuERROR;
 		status = DFU_STATUS_errADDRESS;
@@ -232,9 +261,9 @@ static __dfufunc int handle_upload(u_int16_t val, u_int16_t len)
 		return -EINVAL;
 	}
 
-	if (ptr + len > AT91C_IFLASH + AT91C_IFLASH_SIZE) 
-		len = AT91C_IFLASH + AT91C_IFLASH_SIZE - (u_int32_t) ptr;
-		
+	if (ptr + len > AT91C_IFLASH_END)
+		len = AT91C_IFLASH_END - (u_int8_t *)ptr;
+
 	udp_ep0_send_data((char *)ptr, len);
 	ptr+= len;
 
@@ -246,21 +275,26 @@ static __dfufunc void handle_getstatus(void)
 	struct dfu_status dstat;
 	u_int32_t fsr = AT91F_MC_EFC_GetStatus(AT91C_BASE_MC);
 
-	DEBUGE("getstatus ");
+	DEBUGE("getstatus(fsr=0x%08x) ", fsr);
 
 	switch (dfu_state) {
 	case DFU_STATE_dfuDNLOAD_SYNC:
 	case DFU_STATE_dfuDNBUSY:
 		if (fsr & AT91C_MC_PROGE) {
+			DEBUGE("errPROG ");
 			status = DFU_STATUS_errPROG;
 			dfu_state = DFU_STATE_dfuERROR;
 		} else if (fsr & AT91C_MC_LOCKE) {
+			DEBUGE("errWRITE ");
 			status = DFU_STATUS_errWRITE;
 			dfu_state = DFU_STATE_dfuERROR;
-		} else if (fsr & AT91C_MC_FRDY)
+		} else if (fsr & AT91C_MC_FRDY) {
+			DEBUGE("DNLOAD_IDLE ");
 			dfu_state = DFU_STATE_dfuDNLOAD_IDLE;
-		else
+		} else {
+			DEBUGE("DNBUSY ");
 			dfu_state = DFU_STATE_dfuDNBUSY;
+		}
 		break;
 	case DFU_STATE_dfuMANIFEST_SYNC:
 		dfu_state = DFU_STATE_dfuMANIFEST;
@@ -288,16 +322,18 @@ static void __dfufunc handle_getstate(void)
 int __dfufunc dfu_ep0_handler(u_int8_t req_type, u_int8_t req,
 		    u_int16_t val, u_int16_t len)
 {
-	int rc;
+	int rc, ret = RET_NOTHING;
 
 	DEBUGE("old_state = %u ", dfu_state);
 
 	switch (dfu_state) {
 	case DFU_STATE_appIDLE:
-		if (req != USB_REQ_DFU_DETACH)
-			goto send_stall;
+		if (req != USB_REQ_DFU_DETACH) {
+			ret = RET_STALL;
+			goto out;
+		}
 		dfu_state = DFU_STATE_appDETACH;
-		goto send_zlp;
+		ret = RET_ZLP;
 		break;
 	case DFU_STATE_appDETACH:
 		switch (req) {
@@ -309,7 +345,8 @@ int __dfufunc dfu_ep0_handler(u_int8_t req_type, u_int8_t req,
 			break;
 		default:
 			dfu_state = DFU_STATE_appIDLE;
-			goto send_stall;
+			ret = RET_STALL;
+			goto out;
 			break;
 		}
 		/* FIXME: implement timer to return to appIDLE */
@@ -319,10 +356,11 @@ int __dfufunc dfu_ep0_handler(u_int8_t req_type, u_int8_t req,
 		case USB_REQ_DFU_DNLOAD:
 			if (len == 0) {
 				dfu_state = DFU_STATE_dfuERROR;
-				goto send_stall;
+				ret = RET_STALL;
+				goto out;
 			}
 			ptr = (u_int8_t *) AT91C_IFLASH + SAM7DFU_SIZE;
-			handle_dnload(val, len);
+			ret = handle_dnload(val, len);
 			dfu_state = DFU_STATE_dfuDNLOAD_SYNC;
 			break;
 		case USB_REQ_DFU_UPLOAD:
@@ -332,7 +370,7 @@ int __dfufunc dfu_ep0_handler(u_int8_t req_type, u_int8_t req,
 			break;
 		case USB_REQ_DFU_ABORT:
 			/* no zlp? */
-			goto send_zlp;
+			ret = RET_ZLP;
 			break;
 		case USB_REQ_DFU_GETSTATUS:
 			handle_getstatus();
@@ -342,7 +380,8 @@ int __dfufunc dfu_ep0_handler(u_int8_t req_type, u_int8_t req,
 			break;
 		default:
 			dfu_state = DFU_STATE_dfuERROR;
-			goto send_stall;
+			ret = RET_STALL;
+			goto out;
 			break;
 		}
 		break;
@@ -357,21 +396,32 @@ int __dfufunc dfu_ep0_handler(u_int8_t req_type, u_int8_t req,
 			break;
 		default:
 			dfu_state = DFU_STATE_dfuERROR;
-			goto send_stall;
+			ret = RET_STALL;
+			goto out;
 		}
 		break;
 	case DFU_STATE_dfuDNBUSY:
-		dfu_state = DFU_STATE_dfuERROR;
-		goto send_stall;
+		switch (req) {
+		case USB_REQ_DFU_GETSTATUS:
+			/* FIXME: only accept getstatus if bwPollTimeout
+			 * has elapsed */
+			handle_getstatus();
+			break;
+		default:
+			dfu_state = DFU_STATE_dfuERROR;
+			ret = RET_STALL;
+			goto out;
+		}
 		break;
 	case DFU_STATE_dfuDNLOAD_IDLE:
 		switch (req) {
 		case USB_REQ_DFU_DNLOAD:
-			handle_dnload(val, len);
+			ret = handle_dnload(val, len);
+			dfu_state = DFU_STATE_dfuDNLOAD_SYNC;
 			break;
 		case USB_REQ_DFU_ABORT:
 			dfu_state = DFU_STATE_dfuIDLE;
-			goto send_zlp;
+			ret = RET_ZLP;
 			break;
 		case USB_REQ_DFU_GETSTATUS:
 			handle_getstatus();
@@ -381,7 +431,7 @@ int __dfufunc dfu_ep0_handler(u_int8_t req_type, u_int8_t req,
 			break;
 		default:
 			dfu_state = DFU_STATE_dfuERROR;
-			goto send_stall;
+			ret = RET_STALL;
 			break;
 		}
 		break;
@@ -395,13 +445,13 @@ int __dfufunc dfu_ep0_handler(u_int8_t req_type, u_int8_t req,
 			break;
 		default:
 			dfu_state = DFU_STATE_dfuERROR;
-			goto send_stall;
+			ret = RET_STALL;
 			break;
 		}
 		break;
 	case DFU_STATE_dfuMANIFEST:
 		dfu_state = DFU_STATE_dfuERROR;
-		goto send_stall;
+		ret = RET_STALL;
 		break;
 	case DFU_STATE_dfuMANIFEST_WAIT_RST:
 		/* we should never go here */
@@ -417,7 +467,7 @@ int __dfufunc dfu_ep0_handler(u_int8_t req_type, u_int8_t req,
 		case USB_REQ_DFU_ABORT:
 			dfu_state = DFU_STATE_dfuIDLE;
 			/* no zlp? */
-			goto send_zlp;
+			ret = RET_ZLP;
 			break;
 		case USB_REQ_DFU_GETSTATUS:
 			handle_getstatus();
@@ -427,7 +477,7 @@ int __dfufunc dfu_ep0_handler(u_int8_t req_type, u_int8_t req,
 			break;
 		default:
 			dfu_state = DFU_STATE_dfuERROR;
-			goto send_stall;
+			ret = RET_STALL;
 			break;
 		}
 		break;
@@ -443,29 +493,32 @@ int __dfufunc dfu_ep0_handler(u_int8_t req_type, u_int8_t req,
 			dfu_state = DFU_STATE_dfuIDLE;
 			status = DFU_STATUS_OK;
 			/* no zlp? */
-			goto send_zlp;
+			ret = RET_ZLP;
 			break;
 		default:
 			dfu_state = DFU_STATE_dfuERROR;
-			goto send_stall;
+			ret = RET_STALL;
 			break;
 		}
 		break;
 	}
 
-	DEBUGE("OK new_state = %u\r\n", dfu_state);
-	return 0;
-
-send_stall:
-	udp_ep0_send_stall();
-	DEBUGE("STALL new_state = %u\r\n", dfu_state);
-	return -EINVAL;
+out:
+	DEBUGE("new_state = %u\r\n", dfu_state);
 
-send_zlp:
-	udp_ep0_send_zlp();
-	DEBUGE("ZLP new_state = %u\r\n", dfu_state);
+	switch (ret) {
+	case RET_NOTHING:
+		break;
+	case RET_ZLP:
+		udp_ep0_send_zlp();
+		break;
+	case RET_STALL:
+		udp_ep0_send_stall();
+		break;
+	}
 	return 0;
 }
+
 static u_int8_t cur_config;
 
 /* USB DFU Device descriptor in DFU mode */
@@ -548,13 +601,13 @@ static __dfufunc void dfu_udp_ep0_handler(void)
 	}
 
 	if (!(csr & AT91C_UDP_RXSETUP)) {
-		DEBUGE("no setup packet ");
+		DEBUGE("no setup packet\r\n");
 		return;
 	}
 
 	DEBUGE("len=%d ", csr >> 16);
 	if (csr >> 16  == 0) {
-		DEBUGE("empty packet ");
+		DEBUGE("empty packet\r\n");
 		return;
 	}
 
diff --git a/firmware/src/dfu/flash.c b/firmware/src/dfu/flash.c
index 1a40627..4d0c9d9 100644
--- a/firmware/src/dfu/flash.c
+++ b/firmware/src/dfu/flash.c
@@ -2,22 +2,23 @@
 #include <lib_AT91SAM7.h>
 #include <AT91SAM7.h>
 #include <dfu/dbgu.h>
+#include <board.h>
 
-#define EFCS_CMD_WRITE_PAGE		0x01
-#define EFCS_CMD_SET_LOCK_BIT		0x02
-#define EFCS_CMD_WRITE_PAGE_LOCK	0x03
-#define EFCS_CMD_CLEAR_LOCK		0x04
-#define EFCS_CMD_ERASE_ALL		0x08
-#define EFCS_CMD_SET_NVM_BIT		0x0b
-#define EFCS_CMD_CLEAR_NVM_BIT		0x0d
-#define EFCS_CMD_SET_SECURITY_BIT	0x0f
+#define EFCS_CMD_WRITE_PAGE		0x1
+#define EFCS_CMD_SET_LOCK_BIT		0x2
+#define EFCS_CMD_WRITE_PAGE_LOCK	0x3
+#define EFCS_CMD_CLEAR_LOCK		0x4
+#define EFCS_CMD_ERASE_ALL		0x8
+#define EFCS_CMD_SET_NVM_BIT		0xb
+#define EFCS_CMD_CLEAR_NVM_BIT		0xd
+#define EFCS_CMD_SET_SECURITY_BIT	0xf
 
 
 static u_int16_t page_from_ramaddr(const void *addr)
 {
 	u_int32_t ramaddr = (u_int32_t) addr;
 	ramaddr -= (u_int32_t) AT91C_IFLASH;
-	return (ramaddr >> AT91C_IFLASH_PAGE_SHIFT);
+	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)
@@ -32,13 +33,15 @@ static int is_page_locked(u_int16_t page)
 
 static void unlock_page(u_int16_t page)
 {
+	page &= 0x3ff;
 	AT91F_MC_EFC_PerformCmd(AT91C_BASE_MC, AT91C_MC_FCMD_UNLOCK |
-				AT91C_MC_CORRECT_KEY | page);
+				AT91C_MC_CORRECT_KEY | (page << 8));
 }
 
 void flash_page(u_int8_t *addr)
 {
-	u_int16_t page = page_from_ramaddr(addr);
+	u_int16_t page = page_from_ramaddr(addr) & 0x3ff;
+	u_int32_t fsr = AT91F_MC_EFC_GetStatus(AT91C_BASE_MC);
 	DEBUGP("flash_page(0x%x=%u) ", addr, page);
 
 	if (is_page_locked(page)) {
@@ -46,9 +49,23 @@ void flash_page(u_int8_t *addr)
 		unlock_page(page);
 	}
 
+	if (!(fsr & AT91C_MC_FRDY)) {
+		DEBUGP("NOT_FLASHING ");		
+		return;
+	}
+
 	DEBUGP("performing start_prog ");
+#if 1
 	AT91F_MC_EFC_PerformCmd(AT91C_BASE_MC, AT91C_MC_FCMD_START_PROG |
-				AT91C_MC_CORRECT_KEY | page);
+				AT91C_MC_CORRECT_KEY | (page << 8));
+#endif
 }
 
+void flash_init(void)
+{
+	unsigned int fmcn = AT91F_MC_EFC_ComputeFMCN(MCK);
 
+	AT91F_MC_EFC_CfgModeReg(AT91C_BASE_MC, fmcn << 16 | AT91C_MC_FWS_3FWS |
+				AT91C_MC_FRDY | AT91C_MC_LOCKE | 
+				AT91C_MC_PROGE);
+}
diff --git a/firmware/src/dfu/flash.h b/firmware/src/dfu/flash.h
index 0e6e781..b812714 100644
--- a/firmware/src/dfu/flash.h
+++ b/firmware/src/dfu/flash.h
@@ -1,4 +1,5 @@
 #ifndef _FLASH_H
 #define _FLASH_H
 extern void flash_page(u_int8_t *addr);
+extern void flash_init(void);
 #endif
diff --git a/firmware/src/start/Cstartup.S b/firmware/src/start/Cstartup.S
index 6861750..b154672 100644
--- a/firmware/src/start/Cstartup.S
+++ b/firmware/src/start/Cstartup.S
@@ -84,8 +84,15 @@
 	.equ AT91C_BASE_PIOA, (0xFFFFF400)
 	.equ AT91C_ID_PIOA,   (2)
 	.equ PIOA_PDSR,	      (0x3c)
+#if defined(PCD)
 	.equ PIO_BOOTLDR,     (1 << 27)
-	#.equ PIO_BOOTLDR,     (1 << 6)
+#elif defined(PICC)
+	.equ PIO_BOOTLDR,     (1 << 6)
+#elif defined(OLIMEX)
+	.equ PIO_BOOTLDR,     (1 << 19)
+#else
+#error please define PIO_BOOTLDR
+#endif
 
 
 /* #include "AT91SAM7S64_inc.h"	  */