X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fsamd21%2Fao_flash_samd21.c;h=502ff35e07710b680972f075f6254709c71975d8;hb=master;hp=c1cee3f876e75e95b493e24866cd41e48a8cfee9;hpb=5e7e4303f161e38ac2d84110dccafc67b3aa7fc8;p=fw%2Faltos

diff --git a/src/samd21/ao_flash_samd21.c b/src/samd21/ao_flash_samd21.c
index c1cee3f8..502ff35e 100644
--- a/src/samd21/ao_flash_samd21.c
+++ b/src/samd21/ao_flash_samd21.c
@@ -38,7 +38,15 @@ samd21_nvmctrl_lock_region(void)
 static uint8_t
 ao_flash_lock_region_bit(void *addr)
 {
-	return (uint8_t) (((uintptr_t) addr) / samd21_nvmctrl_lock_region());
+	uint32_t lock_region = samd21_nvmctrl_lock_region();
+	uintptr_t a = (uintptr_t) addr;
+
+	while (lock_region) {
+		a >>= 1;
+		lock_region >>= 1;
+	}
+
+	return (uint8_t) a;
 }
 
 static uint8_t
@@ -101,6 +109,9 @@ ao_flash_erase_page(uint32_t *page)
 	uint32_t row_size = samd21_nvmctrl_row_size();
 	uint32_t rows = (row_size + 255) / 256;
 
+	if ((uintptr_t) page & (row_size - 1))
+		return;
+
 	ao_arch_block_interrupts();
 
 	if (((uintptr_t) row & (row_size - 1)) == 0) {
@@ -116,10 +127,12 @@ ao_flash_erase_page(uint32_t *page)
 void
 ao_flash_page(uint32_t *page, uint32_t *src)
 {
-	uint32_t	page_size = samd21_nvmctrl_page_size();
-	uint32_t	pages = 256 / page_size;
+	uint32_t	page_shift = samd21_nvmctrl_page_shift();
+	uint32_t	pages = 256 >> page_shift;
 	uint32_t	i;
-	uint32_t	per_page = page_size / sizeof(uint32_t);
+	uint32_t	per_page = 1 << (page_shift - 2);
+
+	ao_flash_erase_page(page);
 
 	ao_arch_block_interrupts();