[nrf fromtree] soc: nordic: common: dmm: Optimize by using a micro heap

Add micro heap implementation which is using one or more 32 bit masks
to allocate quickly blocks. It is significantly better than using
sys_heap. Difference is especially big on RAM3 heap because heap
control data is in RAM3 space so operations there were extremely
slowly (15 us to allocate a buffer).

Simplified implementation of the heap requires DMM API change as
release functions need to know the length of the allocated buffer as
simple heap requires that (buffer address is enough for the standard
heap).

Signed-off-by: Krzysztof Chruściński <[email protected]>
(cherry picked from commit decdb30)

Author: nordic-krch

soc/nordic/common/Kconfig

@@ -48,5 +48,19 @@ source "subsys/logging/Kconfig.template.log_config"
 
 endif # MRAM_LATENCY
 
+if HAS_NORDIC_DMM
+
+config DMM_HEAP_CHUNKS
+	int "Number of chunks in the DMM heap"
+	default 32
+	help
+	  DMM is using a simplified heap which is using 32 bit mask to allocate
+	  required buffer which consists of contiguous chunks. If there are many
+	  small buffers used with DMM it is possible that allocation will fail.
+	  Number of chunks is a trade-off between performance and granularity.
+	  Must be multiply of 32.
+
+endif # HAS_NORDIC_DMM
+
 rsource "vpr/Kconfig"
 rsource "uicr/Kconfig"

soc/nordic/common/dmm.c

@@ -6,7 +6,7 @@
 #include <string.h>
 #include <zephyr/cache.h>
 #include <zephyr/kernel.h>
-#include <zephyr/sys/sys_heap.h>
+#include <zephyr/sys/bitarray.h>
 #include <zephyr/mem_mgmt/mem_attr.h>
 #include "dmm.h"
 
@@ -26,6 +26,9 @@
 	 .dt_align = DMM_REG_ALIGN_SIZE(node_id),                                                  \
 	 .dt_allc = &_BUILD_LINKER_END_VAR(node_id)},
 
+#define HEAP_NUM_WORDS (CONFIG_DMM_HEAP_CHUNKS / 32)
+BUILD_ASSERT(IS_ALIGNED(CONFIG_DMM_HEAP_CHUNKS, 32));
+
 /* Generate declarations of linker variables used to determine size of preallocated variables
  * stored in memory sections spanning over memory regions.
  * These are used to determine memory left for dynamic bounce buffer allocator to work with.
@@ -42,9 +45,13 @@ struct dmm_region {
 };
 
 struct dmm_heap {
-	struct sys_heap heap;
+	uint32_t mask[HEAP_NUM_WORDS];
+	atomic_t tail_mask[HEAP_NUM_WORDS];
+	uintptr_t ptr;
+	uintptr_t ptr_end;
+	size_t blk_size;
 	const struct dmm_region *region;
-	struct k_spinlock lock;
+	sys_bitarray_t bitarray;
 };
 
 static const struct dmm_region dmm_regions[] = {
@@ -55,7 +62,6 @@ struct {
 	struct dmm_heap dmm_heaps[ARRAY_SIZE(dmm_regions)];
 } dmm_heaps_data;
 
-
 static struct dmm_heap *dmm_heap_find(void *region)
 {
 	struct dmm_heap *dh;
@@ -103,37 +109,144 @@ static bool is_user_buffer_correctly_preallocated(void const *user_buffer, size_
 	return false;
 }
 
-static size_t dmm_heap_start_get(struct dmm_heap *dh)
+/* Function updates the tail bits mask after the allocation. Tail bits are all bits
+ * except the head. Tail bits mask together with a known index of the start of
+ * chunk (because freeing has a buffer address) allows to determine the size of the
+ * buffer (how many chunks were included. Because tail_mask is updated after allocation
+ * we can safely modify bits that represents allocated buffer, we only need to use
+ * atomic operation on the mask since mask may be modified (but different bits).
+ */
+static void tail_mask_set(atomic_t *tail_mask, size_t num_bits, size_t off)
 {
-	return ROUND_UP(dh->region->dt_allc, dh->region->dt_align);
+	size_t tail_bits = num_bits - 1;
+	size_t tail_off = off + 1;
+
+	if (tail_bits == 0) {
+		return;
+	}
+
+	if (HEAP_NUM_WORDS == 1) {
+		atomic_or(tail_mask, BIT_MASK(tail_bits) << tail_off);
+		return;
+	}
+
+	/* If bit mask exceeds a single word then tail may spill to the adjacent word. */
+	size_t idx = tail_off / 32;
+
+	tail_off = tail_off - 32 * idx;
+	if ((tail_off + tail_bits) <= 32) {
+		/* Tail mask fits in a single word. */
+		atomic_or(&tail_mask[idx], BIT_MASK(tail_bits) << tail_off);
+		return;
+	}
+
+	/* Tail spilled. Remainder is set in the next word. Since number of tail_masks
+	 * match number of words in bitarray we don't need to check if we are exceeding
+	 * the array boundary.
+	 */
+	atomic_or(&tail_mask[idx], BIT_MASK(32 - tail_off) << tail_off);
+
+
+	size_t rem_tail = tail_bits - (32 - tail_off);
+	atomic_t *mask = &tail_mask[idx + 1];
+
+	while (rem_tail >= 32) {
+		atomic_or(mask, UINT32_MAX);
+		mask++;
+		rem_tail -= 32;
+	}
+	atomic_or(mask, BIT_MASK(rem_tail));
 }
 
-static size_t dmm_heap_size_get(struct dmm_heap *dh)
+/* Function determines how many chunks were used for the allocated buffer. It is
+ * determined from tail bits mask and index of the starting chunk (%p off).
+ * Function is called before bits are freed in the bitarray so we can safely modify
+ * bits that belong to that buffer.
+ *
+ * @param tail_mask Pointer to tail_mask array.
+ * @param off Index of the start of the buffer.
+ *
+ * @return Number of chunks that forms the buffer that will be freed.
+ */
+static uint32_t num_bits_get(atomic_t *tail_mask, size_t off)
 {
-	return (dh->region->dt_size - (dmm_heap_start_get(dh) - dh->region->dt_addr));
+	uint32_t mask;
+	uint32_t num_bits;
+
+	if (HEAP_NUM_WORDS == 1) {
+		mask = (*tail_mask | BIT(off)) >> off;
+		num_bits = (~mask == 0) ? 32 : __builtin_ctz(~mask);
+		if (num_bits > 1) {
+			mask = BIT_MASK(num_bits - 1) << (off + 1);
+			atomic_and(tail_mask, ~mask);
+		}
+
+		return num_bits;
+	}
+
+	/* In multiword bit array we need to check if tail is spilling over to the next word. */
+	size_t idx = off / 32;
+	size_t w_off = off - 32 * idx;
+	atomic_t *t_mask = &tail_mask[idx];
+
+	mask = (*t_mask | BIT(w_off)) >> w_off;
+	num_bits = (~mask == 0) ? 32 : __builtin_ctz(~mask);
+	if (num_bits == 1) {
+		return num_bits;
+	}
+
+	mask = BIT_MASK(num_bits - 1) << (w_off + 1);
+	atomic_and(t_mask, ~mask);
+	if (((w_off + num_bits) == 32) && (idx < (HEAP_NUM_WORDS - 1))) {
+		size_t tmp_bits;
+
+		/* If we are at the end of the one mask we need to check the beginning of the
+		 * next one as there might be remaining part of the tail.
+		 */
+		do {
+			t_mask++;
+			tmp_bits = (*t_mask == UINT32_MAX) ? 32 : __builtin_ctz(~(*t_mask));
+			mask = (tmp_bits == 32) ? UINT32_MAX : BIT_MASK(tmp_bits);
+			atomic_and(t_mask, ~mask);
+			num_bits += tmp_bits;
+		} while ((tmp_bits == 32) && (t_mask != &tail_mask[HEAP_NUM_WORDS - 1]));
+	}
+
+	return num_bits;
 }
 
 static void *dmm_buffer_alloc(struct dmm_heap *dh, size_t length)
 {
-	void *ret;
-	k_spinlock_key_t key;
+	size_t num_bits, off;
+	int rv;
+
+	if (dh->ptr == 0) {
+		/* Not initialized. */
+		return NULL;
+	}
 
 	length = ROUND_UP(length, dh->region->dt_align);
+	num_bits = DIV_ROUND_UP(length, dh->blk_size);
+
+	rv = sys_bitarray_alloc(&dh->bitarray, num_bits, &off);
+	if (rv < 0) {
+		return NULL;
+	}
 
-	key = k_spin_lock(&dh->lock);
-	ret = sys_heap_aligned_alloc(&dh->heap, dh->region->dt_align, length);
-	k_spin_unlock(&dh->lock, key);
+	tail_mask_set(dh->tail_mask, num_bits, off);
 
-	return ret;
+	return (void *)(dh->ptr + dh->blk_size * off);
 }
 
 static void dmm_buffer_free(struct dmm_heap *dh, void *buffer)
 {
-	k_spinlock_key_t key;
+	size_t offset = ((uintptr_t)buffer - dh->ptr) / dh->blk_size;
+	size_t num_bits = num_bits_get(dh->tail_mask, offset);
+	int rv;
 
-	key = k_spin_lock(&dh->lock);
-	sys_heap_free(&dh->heap, buffer);
-	k_spin_unlock(&dh->lock, key);
+	rv = sys_bitarray_free(&dh->bitarray, num_bits, offset);
+	(void)rv;
+	__ASSERT_NO_MSG(rv == 0);
 }
 
 static void dmm_memcpy(void *dst, const void *src, size_t len)
@@ -222,7 +335,7 @@ int dmm_buffer_out_release(void *region, void *buffer_out)
 	/* Check if output buffer is contained within memory area
 	 * managed by dynamic memory allocator
 	 */
-	if (is_buffer_within_region(addr, 0, dmm_heap_start_get(dh), dmm_heap_size_get(dh))) {
+	if (is_buffer_within_region(addr, 0, dh->ptr, dh->ptr_end)) {
 		/* If yes, free the buffer */
 		dmm_buffer_free(dh, buffer_out);
 	}
@@ -309,7 +422,7 @@ int dmm_buffer_in_release(void *region, void *user_buffer, size_t user_length, v
 	/* Check if input buffer is contained within memory area
 	 * managed by dynamic memory allocator
 	 */
-	if (is_buffer_within_region(addr, 0, dmm_heap_start_get(dh), dmm_heap_size_get(dh))) {
+	if (is_buffer_within_region(addr, user_length, dh->ptr, dh->ptr_end)) {
 		/* If yes, free the buffer */
 		dmm_buffer_free(dh, buffer_in);
 	}
@@ -321,11 +434,20 @@ int dmm_buffer_in_release(void *region, void *user_buffer, size_t user_length, v
 int dmm_init(void)
 {
 	struct dmm_heap *dh;
+	int blk_cnt;
+	int heap_space;
 
 	for (size_t idx = 0; idx < ARRAY_SIZE(dmm_regions); idx++) {
 		dh = &dmm_heaps_data.dmm_heaps[idx];
 		dh->region = &dmm_regions[idx];
-		sys_heap_init(&dh->heap, (void *)dmm_heap_start_get(dh), dmm_heap_size_get(dh));
+		dh->ptr = ROUND_UP(dh->region->dt_allc, dh->region->dt_align);
+		heap_space = dh->region->dt_size - (dh->ptr - dh->region->dt_addr);
+		dh->blk_size = ROUND_UP(heap_space / (32 * HEAP_NUM_WORDS), dh->region->dt_align);
+		blk_cnt = heap_space / dh->blk_size;
+		dh->ptr_end = dh->ptr + blk_cnt * dh->blk_size;
+		dh->bitarray.num_bits = blk_cnt;
+		dh->bitarray.num_bundles = HEAP_NUM_WORDS;
+		dh->bitarray.bundles = dh->mask;
 	}
 
 	return 0;