From 033dbae7875e32c04c99c5e83f75b3fd5248e77e Mon Sep 17 00:00:00 2001
From: Marcin Szkudlinski <marcin.szkudlinski@intel.com>
Date: Tue, 15 Mar 2022 13:29:10 +0100
Subject: [PATCH] lib/mem_blocks: add allocation for contiguous memory blocks

Added allocation and freeing of a contiguous memory blocks

Signed-off-by: Marcin Szkudlinski <marcin.szkudlinski@intel.com>
---
 include/sys/mem_blocks.h | 31 ++++++++++++++++
 lib/os/mem_blocks.c      | 77 ++++++++++++++++++++++++++++++++++++++++
 2 files changed, 108 insertions(+)

diff --git a/include/sys/mem_blocks.h b/include/sys/mem_blocks.h
index bbca8944bd..48821ebac5 100644
--- a/include/sys/mem_blocks.h
+++ b/include/sys/mem_blocks.h
@@ -218,6 +218,22 @@ struct sys_multi_mem_blocks {
 int sys_mem_blocks_alloc(sys_mem_blocks_t *mem_block, size_t count,
 			 void **out_blocks);
 
+/**
+ * @brief Allocate a contiguous set of memory blocks
+ *
+ * Allocate multiple memory blocks, and place their pointers into
+ * the output array.
+ *
+ * @param[in]  mem_block Pointer to memory block object.
+ * @param[in]  count     Number of blocks to allocate.
+ * @param[out] out_block Output pointer to the start of the allocated block set
+ *
+ * @retval 0       Successful
+ * @retval -EINVAL Invalid argument supplied.
+ * @retval -ENOMEM Not enough contiguous blocks for allocation.
+ */
+int sys_mem_blocks_alloc_contiguous(sys_mem_blocks_t *mem_block, size_t count,
+				   void **out_block);
 
 /**
  * @brief Force allocation of a specified blocks in a memory block object
@@ -254,6 +270,21 @@ int sys_mem_blocks_get(sys_mem_blocks_t *mem_block, void *in_block, size_t count
 int sys_mem_blocks_free(sys_mem_blocks_t *mem_block, size_t count,
 			void **in_blocks);
 
+/**
+ * @brief Free contiguous multiple memory blocks
+ *
+ * Free contiguous multiple memory blocks
+ *
+ * @param[in] mem_block Pointer to memory block object.
+ * @param[in] block     Pointer to the first memory block
+ * @param[in] count     Number of blocks to free.
+ *
+ * @retval 0       Successful
+ * @retval -EINVAL Invalid argument supplied.
+ * @retval -EFAULT Invalid pointer supplied.
+ */
+int sys_mem_blocks_free_contiguous(sys_mem_blocks_t *mem_block, void *block, size_t count);
+
 /**
  * @brief Initialize multi memory blocks allocator group
  *
diff --git a/lib/os/mem_blocks.c b/lib/os/mem_blocks.c
index b0ad5ab05c..6725a73406 100644
--- a/lib/os/mem_blocks.c
+++ b/lib/os/mem_blocks.c
@@ -57,6 +57,45 @@ out:
 	return ret;
 }
 
+int sys_mem_blocks_alloc_contiguous(sys_mem_blocks_t *mem_block, size_t count,
+				   void **out_block)
+{
+	int ret = 0;
+
+	if ((mem_block == NULL) || (out_block == NULL)) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (count == 0) {
+		/* Nothing to allocate */
+		*out_block = NULL;
+		goto out;
+	}
+
+	if (count > mem_block->num_blocks) {
+		/* Definitely not enough blocks to be allocated */
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	void *ptr = alloc_blocks(mem_block, count);
+
+	if (ptr == NULL) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	*out_block = ptr;
+#ifdef CONFIG_SYS_MEM_BLOCKS_LISTENER
+	heap_listener_notify_alloc(HEAP_ID_FROM_POINTER(mem_block),
+				   ptr, count << mem_block->blk_sz_shift);
+#endif
+
+out:
+		return ret;
+}
+
 int sys_mem_blocks_alloc(sys_mem_blocks_t *mem_block, size_t count,
 			 void **out_blocks)
 {
@@ -204,6 +243,44 @@ out:
 	return ret;
 }
 
+int sys_mem_blocks_free_contiguous(sys_mem_blocks_t *mem_block, void *block, size_t count)
+{
+	int ret = 0;
+
+	if (mem_block == NULL) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	CHECKIF((mem_block->bitmap == NULL) || (mem_block->buffer == NULL)) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (count == 0) {
+		/* Nothing to be freed. */
+		goto out;
+	}
+
+	if (count > mem_block->num_blocks) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	ret = free_blocks(mem_block, block, count);
+
+	if (ret != 0) {
+		goto out;
+	}
+#ifdef CONFIG_SYS_MEM_BLOCKS_LISTENER
+	heap_listener_notify_free(HEAP_ID_FROM_POINTER(mem_block),
+			block, count << mem_block->blk_sz_shift);
+#endif
+
+out:
+	return ret;
+}
+
 void sys_multi_mem_blocks_init(sys_multi_mem_blocks_t *group,
 			       sys_multi_mem_blocks_choice_fn_t choice_fn)
 {