Updated for samd51 flash storage compatibility

src/samd_platform.cpp

@@ -87,7 +87,11 @@ void SamdPlatform::init()
     _memoryType = Flash;
     _pageSize = pageSizes[NVMCTRL->PARAM.bit.PSZ];
     _pageCnt = NVMCTRL->PARAM.bit.NVMP;
-    _rowSize = PAGES_PER_ROW * _pageSize;
+    #if defined(__SAMD51__)
+        _rowSize = (_pageSize * _pageCnt / 64);
+    #else
+        _rowSize = (_pageSize * PAGES_PER_ROW);
+    #endif
 
     // find end of program flash and set limit to next row
     uint32_t endEddr = (uint32_t)(&__etext + (&__data_end__ - &__data_start__)); // text + data MemoryBlock
@@ -100,6 +104,33 @@ void SamdPlatform::init()
     }
 }
 
+#if defined(__SAMD51__)
+// Invalidate all CMCC cache entries if CMCC cache is enabled.
+static void invalidate_CMCC_cache()
+{
+  if (CMCC->SR.bit.CSTS) {
+    CMCC->CTRL.bit.CEN = 0;
+    while (CMCC->SR.bit.CSTS) {}
+    CMCC->MAINT0.bit.INVALL = 1;
+    CMCC->CTRL.bit.CEN = 1;
+  }
+}
+#endif
+
+static inline uint32_t read_unaligned_uint32(volatile void *data)
+{
+  union {
+    uint32_t u32;
+    uint8_t u8[4];
+  } res;
+  const uint8_t *d = (const uint8_t *)data;
+  res.u8[0] = d[0];
+  res.u8[1] = d[1];
+  res.u8[2] = d[2];
+  res.u8[3] = d[3];
+  return res.u32;
+}
+
 size_t SamdPlatform::flashEraseBlockSize()
 {
     return PAGES_PER_ROW;
@@ -171,36 +202,54 @@ void SamdPlatform::write(const volatile void *flash_ptr, const void *data, uint3
     size = (size + 3) / 4;
     volatile uint32_t *src_addr = (volatile uint32_t *)data;
     volatile uint32_t *dst_addr = (volatile uint32_t *)flash_ptr;
-    // volatile uint32_t *dst_addr = (volatile uint32_t *)flash_ptr;
-    // const uint8_t *src_addr = (uint8_t *)data;
 
     // Disable automatic page write
+	#if defined(__SAMD51__)
+		NVMCTRL->CTRLA.bit.WMODE = 0;
+		while (NVMCTRL->STATUS.bit.READY == 0) { }
+		// Disable NVMCTRL cache while writing, per SAMD51 errata.
+		bool original_CACHEDIS0 = NVMCTRL->CTRLA.bit.CACHEDIS0;
+		bool original_CACHEDIS1 = NVMCTRL->CTRLA.bit.CACHEDIS1;
+		NVMCTRL->CTRLA.bit.CACHEDIS0 = true;
+		NVMCTRL->CTRLA.bit.CACHEDIS1 = true;
+	#else
 		NVMCTRL->CTRLB.bit.MANW = 1;
+	#endif
 
     // Do writes in pages
     while (size)
     {
         // Execute "PBC" Page Buffer Clear
+        #if defined(__SAMD51__)
+            NVMCTRL->CTRLB.reg = NVMCTRL_CTRLB_CMDEX_KEY | NVMCTRL_CTRLB_CMD_PBC;
+            while (NVMCTRL->INTFLAG.bit.DONE == 0) { }
+        #else
             NVMCTRL->CTRLA.reg = NVMCTRL_CTRLA_CMDEX_KEY | NVMCTRL_CTRLA_CMD_PBC;
-        while (NVMCTRL->INTFLAG.bit.READY == 0)
+            while (NVMCTRL->INTFLAG.bit.READY == 0) { }
-        {
+        #endif
-        }
 
         // Fill page buffer
         uint32_t i;
         for (i = 0; i < (_pageSize / 4) && size; i++)
         {
-            *dst_addr = *src_addr;
+            *dst_addr = read_unaligned_uint32(src_addr);
-            src_addr++;
+            src_addr += 4;
             dst_addr++;
             size--;
         }
 
         // Execute "WP" Write Page
+        #if defined(__SAMD51__)
+            NVMCTRL->CTRLB.reg = NVMCTRL_CTRLB_CMDEX_KEY | NVMCTRL_CTRLB_CMD_WP;
+            while (NVMCTRL->INTFLAG.bit.DONE == 0) { }
+            invalidate_CMCC_cache();
+            // Restore original NVMCTRL cache settings.
+            NVMCTRL->CTRLA.bit.CACHEDIS0 = original_CACHEDIS0;
+            NVMCTRL->CTRLA.bit.CACHEDIS1 = original_CACHEDIS1;
+        #else
             NVMCTRL->CTRLA.reg = NVMCTRL_CTRLA_CMDEX_KEY | NVMCTRL_CTRLA_CMD_WP;
-        while (NVMCTRL->INTFLAG.bit.READY == 0)
+            while (NVMCTRL->INTFLAG.bit.READY == 0) { }
-        {
+        #endif
-        }
     }
 }
 
@@ -218,11 +267,16 @@ void SamdPlatform::erase(const volatile void *flash_ptr, uint32_t size)
 
 void SamdPlatform::eraseRow(const volatile void *flash_ptr)
 {
+    #if defined(__SAMD51__)
+        NVMCTRL->ADDR.reg = ((uint32_t)flash_ptr);
+        NVMCTRL->CTRLB.reg = NVMCTRL_CTRLB_CMDEX_KEY | NVMCTRL_CTRLB_CMD_EB;
+        while (!NVMCTRL->INTFLAG.bit.DONE) { }
+        invalidate_CMCC_cache();
+    #else
         NVMCTRL->ADDR.reg = ((uint32_t)flash_ptr) / 2;
         NVMCTRL->CTRLA.reg = NVMCTRL_CTRLA_CMDEX_KEY | NVMCTRL_CTRLA_CMD_ER;
-    while (!NVMCTRL->INTFLAG.bit.READY)
+        while (!NVMCTRL->INTFLAG.bit.READY) { }
-    {
+    #endif
-    }
 }
 
 #endif