From 8ca5b7bd205764d80725fa77e0c2d2153391a478 Mon Sep 17 00:00:00 2001
From: Jaroslaw Stelter <Jaroslaw.Stelter@intel.com>
Date: Tue, 12 Dec 2023 12:26:49 +0100
Subject: [PATCH] zephyr: lib: alloc: Use cached memory for L3 Heap

This is recommended HW flow for Intel ADSP code.
L3 Heap should be accessed thought cached pointers
including its management data.

Signed-off-by: Jaroslaw Stelter <Jaroslaw.Stelter@intel.com>
---
 zephyr/lib/alloc.c | 47 ++++++++++++++++++++++++++++++++++++++++------
 1 file changed, 41 insertions(+), 6 deletions(-)

diff --git a/zephyr/lib/alloc.c b/zephyr/lib/alloc.c
index f20e125e9246..8879a58bd426 100644
--- a/zephyr/lib/alloc.c
+++ b/zephyr/lib/alloc.c
@@ -115,8 +115,8 @@ static inline uintptr_t get_l3_heap_start(void)
 	 * - main_fw_load_offset
 	 * - main fw size in manifest
 	 */
-	return (uintptr_t)z_soc_uncached_ptr((__sparse_force void __sparse_cache *)
-					     ROUND_UP(IMR_L3_HEAP_BASE, L3_MEM_PAGE_SIZE));
+	return (uintptr_t)((__sparse_force void __sparse_cache *)
+			   ROUND_UP(IMR_L3_HEAP_BASE, L3_MEM_PAGE_SIZE));
 }
 
 /**
@@ -144,14 +144,29 @@ static bool is_l3_heap_pointer(void *ptr)
 	uintptr_t l3_heap_start = get_l3_heap_start();
 	uintptr_t l3_heap_end = l3_heap_start + get_l3_heap_size();
 
-	if (is_cached(ptr))
-		ptr = z_soc_uncached_ptr((__sparse_force void __sparse_cache *)ptr);
-
 	if ((POINTER_TO_UINT(ptr) >= l3_heap_start) && (POINTER_TO_UINT(ptr) < l3_heap_end))
 		return true;
 
 	return false;
 }
+/**
+ * Flush cached first page of of L3 memory heap.
+ * It contains L3 heap management data.
+ */
+static inline void l3_heap_flush(struct k_heap *h)
+{
+	if (h == &l3_heap)
+		sys_cache_data_flush_and_invd_range((void *)get_l3_heap_start(),
+						    L3_MEM_PAGE_SIZE);
+}
+
+static inline void l3_heap_invalidate(struct k_heap *h)
+{
+	if (h == &l3_heap)
+		sys_cache_data_invd_range((void *)get_l3_heap_start(),
+					  L3_MEM_PAGE_SIZE);
+}
+
 #endif
 
 static void *heap_alloc_aligned(struct k_heap *h, size_t min_align, size_t bytes)
@@ -163,7 +178,13 @@ static void *heap_alloc_aligned(struct k_heap *h, size_t min_align, size_t bytes
 #endif
 
 	key = k_spin_lock(&h->lock);
+#if CONFIG_L3_HEAP
+	l3_heap_invalidate(h);
+#endif
 	ret = sys_heap_aligned_alloc(&h->heap, min_align, bytes);
+#if CONFIG_L3_HEAP
+	l3_heap_flush(h);
+#endif
 	k_spin_unlock(&h->lock, key);
 
 #if CONFIG_SYS_HEAP_RUNTIME_STATS && CONFIG_IPC_MAJOR_4
@@ -212,6 +233,10 @@ static void heap_free(struct k_heap *h, void *mem)
 
 	if (is_cached(mem)) {
 		mem_uncached = z_soc_uncached_ptr((__sparse_force void __sparse_cache *)mem);
+#if CONFIG_L3_HEAP
+		if (h == &l3_heap)
+			mem_uncached = (__sparse_force void __sparse_cache *)mem;
+#endif
 		sys_cache_data_flush_and_invd_range(mem,
 				sys_heap_usable_size(&h->heap, mem_uncached));
 
@@ -219,8 +244,13 @@ static void heap_free(struct k_heap *h, void *mem)
 	}
 #endif
 
+#if CONFIG_L3_HEAP
+	l3_heap_invalidate(h);
+#endif
 	sys_heap_free(&h->heap, mem);
-
+#if CONFIG_L3_HEAP
+	l3_heap_flush(h);
+#endif
 	k_spin_unlock(&h->lock, key);
 }
 
@@ -250,6 +280,9 @@ void *rmalloc(enum mem_zone zone, uint32_t flags, uint32_t caps, size_t bytes)
 	if (caps & SOF_MEM_CAPS_L3) {
 #if CONFIG_L3_HEAP
 		heap = &l3_heap;
+		/* Uncached L3_HEAP should be not used */
+		if (!zone_is_cached(zone))
+			k_panic();
 #else
 		k_panic();
 #endif
@@ -341,6 +374,7 @@ void *rballoc_align(uint32_t flags, uint32_t caps, size_t bytes,
 /*
  * Free's memory allocated by above alloc calls.
  */
+__attribute__((optimize("-O0")))
 void rfree(void *ptr)
 {
 	if (!ptr)
@@ -362,6 +396,7 @@ static int heap_init(void)
 
 #if CONFIG_L3_HEAP
 	sys_heap_init(&l3_heap.heap, UINT_TO_POINTER(get_l3_heap_start()), get_l3_heap_size());
+	l3_heap_flush(&l3_heap);
 #endif
 
 	return 0;