From 5368415c9a9ff9b142ac4559fccf0f5e61d96f6b Mon Sep 17 00:00:00 2001
From: fjmolinas <femolina@uc.cl>
Date: Sun, 4 Aug 2019 11:39:27 +0200
Subject: [PATCH] cpu/msp430_common: add flashpage_raw support

---
 cpu/msp430_common/Makefile.features  |  1 +
 cpu/msp430_common/include/cpu_conf.h |  7 +++
 cpu/msp430_common/periph/flashpage.c | 89 +++++++++++++++++++++-------
 3 files changed, 75 insertions(+), 22 deletions(-)

diff --git a/cpu/msp430_common/Makefile.features b/cpu/msp430_common/Makefile.features
index 15cb270bb0..cf5de6918c 100644
--- a/cpu/msp430_common/Makefile.features
+++ b/cpu/msp430_common/Makefile.features
@@ -1,2 +1,3 @@
 FEATURES_PROVIDED += periph_flashpage
+FEATURES_PROVIDED += periph_flashpage_raw
 FEATURES_PROVIDED += periph_pm
diff --git a/cpu/msp430_common/include/cpu_conf.h b/cpu/msp430_common/include/cpu_conf.h
index 67b9ec4130..738652bf9d 100644
--- a/cpu/msp430_common/include/cpu_conf.h
+++ b/cpu/msp430_common/include/cpu_conf.h
@@ -41,6 +41,13 @@ extern "C" {
 #define CPU_FLASH_BASE      (0x8000)
 #define FLASHPAGE_NUMOF     (64U)        /* 32K */
 #endif
+
+/* The minimum block size which can be written is 1B. However, the erase
+ * block is always FLASHPAGE_SIZE.
+ */
+#define FLASHPAGE_RAW_BLOCKSIZE    (1U)
+/* Writing should be always 2 byte aligned */
+#define FLASHPAGE_RAW_ALIGNMENT    (2U)
 /** @} */
 
 /**
diff --git a/cpu/msp430_common/periph/flashpage.c b/cpu/msp430_common/periph/flashpage.c
index 34151df159..29c86998d9 100644
--- a/cpu/msp430_common/periph/flashpage.c
+++ b/cpu/msp430_common/periph/flashpage.c
@@ -24,34 +24,79 @@
 #include "irq.h"
 #include "periph/flashpage.h"
 
+static inline int _unlock(void)
+{
+    int state;
+    state = irq_disable();
+    FCTL3 = FWKEY;
+    while (FCTL3 & BUSY) {}
+    return state;
+}
+
+static inline void _lock(int state)
+{
+    FCTL3 = (FWKEY | LOCK);
+    irq_restore(state);
+}
+
+static inline void _erase(uint16_t *page_addr)
+{
+    /* disable interrupts and unlock flash */
+    int state = _unlock();
+
+    /* erase page */
+    FCTL1 = (FWKEY | ERASE);
+    *page_addr = 0;
+    while (FCTL3 & BUSY) {}
+
+    /* lock flash and re-enable interrupts */
+    _lock(state);
+}
+
+void flashpage_write_raw(void *target_addr, const void *data, size_t len)
+{
+    /* assert multiples of FLASHPAGE_RAW_BLOCKSIZE are written and no less of
+       that length. */
+    assert(!(len % FLASHPAGE_RAW_BLOCKSIZE));
+
+    /* ensure writes are aligned */
+    assert(!(((unsigned)target_addr % FLASHPAGE_RAW_ALIGNMENT) ||
+            ((unsigned)data % FLASHPAGE_RAW_ALIGNMENT)));
+
+    /* ensure the length doesn't exceed the actual flash size */
+    assert(((unsigned)target_addr + len) <=
+           (CPU_FLASH_BASE + (FLASHPAGE_SIZE * FLASHPAGE_NUMOF)));
+
+    uint8_t *page_addr = target_addr;
+    const uint8_t *data_addr = data;
+
+   /* disable interrupts and unlock flash */
+    int state = _unlock();
+
+   /* enable write access, and write*/
+    FCTL1 = (FWKEY | WRT);
+    for (unsigned i = 0; i < len; i++) {
+        *(page_addr++) = *(data_addr++);
+        while (!(FCTL3 & WAIT)) {}
+    }
+   /* disable write access */
+    FCTL1 = (FWKEY);
+
+    /* lock flash and re-enable interrupts */
+    _lock(state);
+}
+
 void flashpage_write(int page, const void *data)
 {
     assert((unsigned) page < FLASHPAGE_NUMOF);
 
-    const uint8_t *src = data;
-    uint8_t *dst = (uint8_t *)flashpage_addr(page);
-    unsigned istate;
-
-    /* disable interrupts and unlock flash */
-    istate = irq_disable();
-    FCTL3 = FWKEY;
-    while (FCTL3 & BUSY) {}
+    uint16_t *page_addr = (uint16_t *)flashpage_addr(page);
 
     /* erase page */
-    FCTL1 = (FWKEY | ERASE);
-    *dst = 0;                   /* erases the page */
-    while (FCTL3 & BUSY) {}
+    _erase(page_addr);
 
-    if (data) {
-        FCTL1 = (FWKEY | WRT);
-        for (unsigned i = 0; i < FLASHPAGE_SIZE; i++) {
-            *(dst++) = *(src++);
-            while (!(FCTL3 & WAIT)) {}
-        }
+    /* write page */
+    if (data != NULL) {
+        flashpage_write_raw(page_addr, data, FLASHPAGE_SIZE);
     }
-
-    /* lock flash and re-enable interrupts */
-    FCTL1 = (FWKEY);
-    FCTL3 = (FWKEY | LOCK);
-    irq_restore(istate);
 }