From 2cbaaa9150c3a3c4cff4e15cbe5b7116a133a523 Mon Sep 17 00:00:00 2001
From: Kristofer Jonsson <kristofer.jonsson@arm.com>
Date: Thu, 19 Nov 2020 16:14:46 +0100
Subject: Improved cache maintenance

Use invalidate by address to avoid invalidating the data segment
loosing data.

Change-Id: Ied36d881d19e56ea760b8775657b0e924e129b31
---
 .../message_process/include/message_process.hpp    |  5 +++
 .../message_process/src/message_process.cc         | 46 ++++++++++++++++------
 2 files changed, 39 insertions(+), 12 deletions(-)

diff --git a/applications/message_process/include/message_process.hpp b/applications/message_process/include/message_process.hpp
index b3c1bdd..602c2a4 100644
--- a/applications/message_process/include/message_process.hpp
+++ b/applications/message_process/include/message_process.hpp
@@ -78,6 +78,11 @@ public:
     }
 
 private:
+    void cleanHeader() const;
+    void cleanHeaderData() const;
+    void invalidateHeader() const;
+    void invalidateHeaderData() const;
+
     ethosu_core_queue &queue;
 };
 
diff --git a/applications/message_process/src/message_process.cc b/applications/message_process/src/message_process.cc
index 5640a1a..200d92b 100644
--- a/applications/message_process/src/message_process.cc
+++ b/applications/message_process/src/message_process.cc
@@ -54,6 +54,8 @@ bool QueueImpl::read(uint8_t *dst, uint32_t length) {
     const uint8_t *end = dst + length;
     uint32_t rpos      = queue.header.read;
 
+    invalidateHeaderData();
+
     if (length > available()) {
         return false;
     }
@@ -65,9 +67,7 @@ bool QueueImpl::read(uint8_t *dst, uint32_t length) {
 
     queue.header.read = rpos;
 
-#if defined(__DCACHE_PRESENT) && (__DCACHE_PRESENT == 1U)
-    SCB_CleanDCache();
-#endif
+    cleanHeader();
 
     return true;
 }
@@ -79,6 +79,8 @@ bool QueueImpl::write(const Vec *vec, size_t length) {
         total += vec[i].length;
     }
 
+    invalidateHeader();
+
     if (total > capacity()) {
         return false;
     }
@@ -98,9 +100,7 @@ bool QueueImpl::write(const Vec *vec, size_t length) {
     // Update the write position last
     queue.header.write = wpos;
 
-#if defined(__DCACHE_PRESENT) && (__DCACHE_PRESENT == 1U)
-    SCB_CleanDCache();
-#endif
+    cleanHeaderData();
 
     return true;
 }
@@ -115,17 +115,43 @@ bool QueueImpl::write(const uint32_t type, const void *src, uint32_t length) {
 bool QueueImpl::skip(uint32_t length) {
     uint32_t rpos = queue.header.read;
 
+    invalidateHeader();
+
     if (length > available()) {
         return false;
     }
 
     queue.header.read = (rpos + length) % queue.header.size;
 
+    cleanHeader();
+
+    return true;
+}
+
+void QueueImpl::cleanHeader() const {
 #if defined(__DCACHE_PRESENT) && (__DCACHE_PRESENT == 1U)
-    SCB_CleanDCache();
+    SCB_CleanDCache_by_Addr(reinterpret_cast<uint32_t *>(&queue.header), sizeof(queue.header));
 #endif
+}
 
-    return true;
+void QueueImpl::cleanHeaderData() const {
+#if defined(__DCACHE_PRESENT) && (__DCACHE_PRESENT == 1U)
+    SCB_CleanDCache_by_Addr(reinterpret_cast<uint32_t *>(&queue.header), sizeof(queue.header));
+    SCB_CleanDCache_by_Addr(reinterpret_cast<uint32_t *>(queue.data), queue.header.size);
+#endif
+}
+
+void QueueImpl::invalidateHeader() const {
+#if defined(__DCACHE_PRESENT) && (__DCACHE_PRESENT == 1U)
+    SCB_InvalidateDCache_by_Addr(reinterpret_cast<uint32_t *>(&queue.header), sizeof(queue.header));
+#endif
+}
+
+void QueueImpl::invalidateHeaderData() const {
+#if defined(__DCACHE_PRESENT) && (__DCACHE_PRESENT == 1U)
+    SCB_InvalidateDCache_by_Addr(reinterpret_cast<uint32_t *>(&queue.header), sizeof(queue.header));
+    SCB_InvalidateDCache_by_Addr(reinterpret_cast<uint32_t *>(queue.data), queue.header.size);
+#endif
 }
 
 MessageProcess::MessageProcess(ethosu_core_queue &in,
@@ -155,10 +181,6 @@ void MessageProcess::handleIrq() {
 bool MessageProcess::handleMessage() {
     ethosu_core_msg msg;
 
-#if defined(__DCACHE_PRESENT) && (__DCACHE_PRESENT == 1U)
-    SCB_InvalidateDCache();
-#endif
-
     // Read msg header
     if (!queueIn.read(msg)) {
         return false;
-- 
cgit v1.2.1