1 /*
2 * Copyright 2023-2024 NXP
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 *
6 */
7
8 #include <stdlib.h>
9 #include "mem_pool.h"
10
11 #if CONFIG_MEM_POOLS
CalculateAndVerifyAlignment(int Alignment)12 static int CalculateAndVerifyAlignment(int Alignment)
13 {
14 /*********************************/
15 int i;
16 int alignmentBit = 0x1;
17 /*********************************/
18
19 /**
20 * Guarantee that the alignment is the size of a pointer.
21 */
22 if (Alignment < (int)sizeof(unsigned char *))
23 {
24 Alignment = (int)sizeof(unsigned char *);
25 }
26
27 for (i = 0; i < 31; i++)
28 {
29 if (Alignment == alignmentBit)
30 {
31 break;
32 }
33 alignmentBit <<= 1;
34 }
35
36 if (i >= 31)
37 {
38 return 0;
39 }
40 else
41 {
42 return Alignment;
43 }
44 }
45
CalculateItemSize(int ItemSize,int Alignment)46 static int CalculateItemSize(int ItemSize, int Alignment)
47 {
48 /*********************************/
49 int alignmentCount;
50 /*********************************/
51
52 if (ItemSize <= Alignment)
53 {
54 /**
55 * The 2* accounts for the SList struct inside each memory block.
56 */
57 ItemSize = 2 * Alignment;
58 }
59 else
60 {
61 alignmentCount = ItemSize / Alignment;
62 if (ItemSize % Alignment != 0)
63 {
64 alignmentCount++;
65 }
66 /**
67 * The +1 accounts for the SList struct inside each memory block.
68 */
69 ItemSize = ((alignmentCount + 1) * Alignment);
70 }
71
72 return ItemSize;
73 }
74
OSA_MemoryPoolCreate(MemPool_t * MemPool,int ItemSize,void * PreallocatedMemory,int PreallocatedMemorySize,int Alignment)75 MemoryPool_t OSA_MemoryPoolCreate(
76 MemPool_t *MemPool, int ItemSize, void *PreallocatedMemory, int PreallocatedMemorySize, int Alignment)
77 {
78 /*********************************/
79 unsigned char *ptr;
80 SlNode_t *Node;
81 osa_status_t status;
82 /*********************************/
83
84 Alignment = CalculateAndVerifyAlignment(Alignment);
85
86 if (Alignment == 0)
87 {
88 return NULL;
89 }
90
91 ItemSize = CalculateItemSize(ItemSize, Alignment);
92
93 status = OSA_MutexCreate((osa_mutex_handle_t)MemPool->Lock);
94 if (status != KOSA_StatusSuccess)
95 {
96 return NULL;
97 }
98
99 InitStack(&MemPool->Stack);
100 MemPool->ItemSize = ItemSize;
101 MemPool->Alignment = Alignment;
102
103 ptr = (unsigned char *)PreallocatedMemory;
104
105 while (PreallocatedMemorySize >= ItemSize)
106 {
107 Node = (SlNode_t *)ptr;
108
109 PushOnStack(&MemPool->Stack, Node);
110 ptr += MemPool->ItemSize;
111 PreallocatedMemorySize -= MemPool->ItemSize;
112 }
113
114 return (MemoryPool_t)MemPool;
115 }
116
OSA_MemoryPoolAllocate(MemoryPool_t pool)117 void *OSA_MemoryPoolAllocate(MemoryPool_t pool)
118 {
119 /*********************************/
120 MemPool_t *MemPool;
121 SlNode_t *Node;
122 unsigned char *ptr;
123 /*********************************/
124
125 MemPool = (MemPool_t *)pool;
126
127 OSA_MutexLock((osa_mutex_handle_t)MemPool->Lock, osaWaitForever_c);
128
129 if (MemPool->Stack.Count == 0)
130 {
131 OSA_MutexUnlock((osa_mutex_handle_t)MemPool->Lock);
132 return NULL;
133 }
134
135 Node = PopOffStack(&MemPool->Stack);
136
137 OSA_MutexUnlock((osa_mutex_handle_t)MemPool->Lock);
138
139 ptr = ((unsigned char *)Node) + MemPool->Alignment;
140
141 return (void *)ptr;
142 }
143
OSA_MemoryPoolFree(MemoryPool_t pool,void * memory)144 void OSA_MemoryPoolFree(MemoryPool_t pool, void *memory)
145 {
146 /*********************************/
147 MemPool_t *MemPool;
148 SlNode_t *Node;
149 unsigned char *ptr;
150 /*********************************/
151
152 MemPool = (MemPool_t *)pool;
153
154 ptr = ((unsigned char *)memory) - MemPool->Alignment;
155
156 Node = (SlNode_t *)ptr;
157
158 OSA_MutexLock((osa_mutex_handle_t)MemPool->Lock, osaWaitForever_c);
159
160 PushOnStack(&MemPool->Stack, Node);
161
162 OSA_MutexUnlock((osa_mutex_handle_t)MemPool->Lock);
163 }
164
165 #endif
166
