1 /* USER CODE BEGIN Header */
2 /**
3 ******************************************************************************
4 * @file dm_alloc.c
5 * @author GPM WBL Application Team
6 * @brief Dynamic Memory Allocator
7 ******************************************************************************
8 * @attention
9 *
10 * Copyright (c) 2024 STMicroelectronics.
11 * All rights reserved.
12 *
13 * This software is licensed under terms that can be found in the LICENSE file
14 * in the root directory of this software component.
15 * If no LICENSE file comes with this software, it is provided AS-IS.
16 *
17 ******************************************************************************
18 */
19 /* USER CODE END Header */
20
21 /******************************************************************************
22 * INCLUDE HEADER FILES
23 *****************************************************************************/
24 #include "dm_alloc.h"
25 #include "osal.h"
26 #include <string.h>
27
28 /******************************************************************************
29 * LOCAL MACROS
30 *****************************************************************************/
31 #define ALIGN_UPTO_32BITS(VAL) (((((unsigned int)(VAL)) - 1U) | (sizeof(uint32_t) - 1U)) + 1U)
32 #define DM_SLICE_THRESHOLD (3 * sizeof(db_alloc_header_t))
33 #define DM_FREE 0x00
34 #define DM_ALLOC 0x01
35 /*#define DM_DEBUG (1) */
36 #if defined(DM_DEBUG)
37 #define DM_DEBUG_STAMP (0xBEAF)
38 #endif
39
40 #define MIN(a,b) (((a) < (b))? (a) : (b))
41
42 /******************************************************************************
43 * LOCAL TYPEDEFS (STRUCTURES, UNIONS, ENUMS)
44 *****************************************************************************/
45 typedef struct db_alloc_header_s {
46 uint16_t buffer_size;
47 uint16_t flags;
48 uint32_t buffer_a[];
49 } db_alloc_header_t;
50
51 typedef struct db_free_header_s {
52 uint16_t buffer_size;
53 uint16_t flags;
54 struct db_free_header_s *next;
55 uint32_t buffer_a[];
56 } dm_free_header_t;
57
58 typedef struct dm_ctx_s {
59 #if defined(DM_DEBUG)
60 uint16_t alloc_size;
61 uint16_t alloc_max_size;
62 #endif
63 dm_free_header_t *head;
64 uint32_t *alloc_space_p;
65 uint16_t buffer_size;
66 } dm_ctx_t;
67
68 /******************************************************************************
69 * LOCAL FUNCTION PROTOTYPES
70 *****************************************************************************/
71 /******************************************************************************
72 * Local Variables
73 *****************************************************************************/
74 static dm_ctx_t dm_ctx;
75
dm_init(uint16_t buffer_size,uint32_t * buffer_p)76 void dm_init(uint16_t buffer_size, uint32_t *buffer_p)
77 {
78 dm_ctx.alloc_space_p = (void *)buffer_p;
79 dm_ctx.head = (dm_free_header_t *)buffer_p;
80 dm_ctx.head->buffer_size = buffer_size;
81 dm_ctx.buffer_size = buffer_size;
82 dm_ctx.head->flags = DM_FREE;
83 #if defined(DM_DEBUG)
84 dm_ctx.alloc_size = 0U;
85 dm_ctx.alloc_max_size = 0U;
86 #endif
87 dm_ctx.head->next = NULL;
88 }
89
db_extract_from_free_list(dm_free_header_t * entry_p)90 static void db_extract_from_free_list(dm_free_header_t *entry_p)
91 {
92 dm_free_header_t *e_p;
93
94 #if defined(DM_DEBUG)
95 if (entry_p->flags != DM_FREE)
96 {
97 while (1)
98 ;
99 }
100 #endif
101 if (entry_p == dm_ctx.head)
102 {
103 dm_ctx.head = entry_p->next;
104 }
105 else
106 {
107 e_p = dm_ctx.head;
108 while (e_p != NULL)
109 {
110 if (e_p->next == entry_p)
111 {
112 e_p->next = entry_p->next;
113 break;
114 }
115 e_p = e_p->next;
116 }
117 }
118 }
119
db_add_to_free_list(dm_free_header_t * free_entry_p)120 static void db_add_to_free_list(dm_free_header_t *free_entry_p)
121 {
122 dm_free_header_t *prev_p;
123
124 if (free_entry_p != NULL)
125 {
126 #if defined(DM_DEBUG)
127 if (free_entry_p->flags != DM_FREE)
128 {
129 while (1)
130 ;
131 }
132 #endif
133 prev_p = NULL;
134 if (dm_ctx.head == NULL)
135 {
136 /**
137 * The free list is empty. Assign the new entry to the list.
138 */
139 dm_ctx.head = free_entry_p;
140 }
141 else
142 {
143 if ((uintptr_t)free_entry_p < (uintptr_t)dm_ctx.head)
144 {
145 /**
146 * Insert the new element at the head of the list.
147 */
148 free_entry_p->next = dm_ctx.head;
149 dm_ctx.head = free_entry_p;
150 }
151 else
152 {
153 prev_p = dm_ctx.head;
154 while (prev_p->next != NULL)
155 {
156 /**
157 * The free list is ordered by index (address) then search
158 * into the list to find the previous node.
159 */
160 if ((uintptr_t)prev_p->next > (uintptr_t)free_entry_p)
161 {
162 break;
163 }
164 prev_p = prev_p->next;
165 }
166
167 /**
168 * Insert the new element.
169 */
170 free_entry_p->next = prev_p->next;
171 prev_p->next = free_entry_p;
172
173 /**
174 * Try to make the new free entry coalesce with the previous.
175 */
176 if (((uintptr_t)prev_p + prev_p->buffer_size) ==
177 (uintptr_t)free_entry_p)
178 {
179 prev_p->next = free_entry_p->next;
180 prev_p->buffer_size += free_entry_p->buffer_size;
181 }
182 }
183
184 /**
185 * Try to make the new free entry coalesce with the next.
186 */
187 if (((uintptr_t)free_entry_p + free_entry_p->buffer_size) ==
188 (uintptr_t)free_entry_p->next)
189 {
190 dm_free_header_t *ne_p;
191
192 ne_p = free_entry_p->next;
193 free_entry_p->next = ne_p->next;
194 free_entry_p->buffer_size += ne_p->buffer_size;
195 }
196 }
197 }
198 }
199
200 /* Release part of a free block and add it to the list of free blocks.
201 Returns the size of the slot not freed. */
dm_slice(dm_free_header_t * free_entry_p,uint16_t min_size)202 static uint16_t dm_slice(dm_free_header_t *free_entry_p, uint16_t min_size)
203 {
204 uint16_t slice_size;
205
206 slice_size = free_entry_p->buffer_size - min_size;
207 if (slice_size > DM_SLICE_THRESHOLD)
208 {
209 dm_free_header_t *slice_p;
210
211 slice_p = (dm_free_header_t *)&free_entry_p->buffer_a[(min_size -
212 sizeof(dm_free_header_t)) >> 2];
213 slice_p->buffer_size = slice_size;
214 slice_p->flags = DM_FREE;
215 slice_p->next = NULL;
216 db_add_to_free_list(slice_p);
217 return min_size;
218 }
219
220 return free_entry_p->buffer_size;
221 }
222
dm_alloc(uint16_t size)223 void *dm_alloc(uint16_t size)
224 {
225 uint16_t alloc_size;
226 dm_free_header_t *entry_p, *best_entry_p;
227
228 best_entry_p = NULL;
229 entry_p = dm_ctx.head;
230 alloc_size = (uint16_t)(ALIGN_UPTO_32BITS(size) + sizeof(db_alloc_header_t));
231 #if defined(DM_DEBUG)
232 if (entry_p != NULL)
233 {
234 if (entry_p->flags != DM_FREE)
235 {
236 while (1)
237 ;
238 }
239 }
240 #endif
241 while (entry_p != NULL)
242 {
243 /**
244 * Best fit strategy: search for the entry that has the size closer to
245 * the requested value.
246 */
247 if (entry_p->buffer_size >= alloc_size)
248 {
249 if ((best_entry_p == NULL) ||
250 ((best_entry_p != NULL) &&
251 (best_entry_p->buffer_size > entry_p->buffer_size)))
252 {
253 best_entry_p = entry_p;
254 }
255 }
256 entry_p = entry_p->next;
257 }
258
259 if (best_entry_p != NULL)
260 {
261 db_alloc_header_t *alloc_entry_p;
262
263 /**
264 * Detach entry by free list.
265 */
266 db_extract_from_free_list(best_entry_p);
267
268 /**
269 * If the extracted entry has a size "much" greater then the
270 * requested one then slice it releasing the not requested space.
271 */
272 best_entry_p->buffer_size = dm_slice(best_entry_p, alloc_size);
273
274 alloc_entry_p = (db_alloc_header_t *)best_entry_p;
275 #if defined(DM_DEBUG)
276 if (alloc_entry_p->flags != DM_FREE)
277 {
278 while (1)
279 ;
280 }
281 dm_ctx.alloc_size += alloc_size + sizeof(uint32_t);
282 if (dm_ctx.alloc_size > dm_ctx.alloc_max_size)
283 {
284 dm_ctx.alloc_max_size = dm_ctx.alloc_size;
285 }
286 #endif
287 best_entry_p->flags = DM_ALLOC;
288
289 return alloc_entry_p->buffer_a;
290 }
291
292 return NULL;
293 }
294
dm_free(void * buffer_p)295 void dm_free(void *buffer_p)
296 {
297 if (buffer_p == NULL)
298 return;
299 dm_free_header_t *free_entry_p;
300 uint32_t *buffer32_p = buffer_p;
301
302 free_entry_p = (dm_free_header_t *)(--buffer32_p);
303 free_entry_p->flags = DM_FREE;
304 free_entry_p->next = NULL;
305 #if defined(DM_DEBUG)
306 dm_ctx.alloc_size -= free_entry_p->buffer_size;
307 #endif
308 db_add_to_free_list(free_entry_p);
309 }
310
dm_realloc(void * buffer_p,uint16_t size)311 void *dm_realloc(void *buffer_p, uint16_t size)
312 {
313 uint16_t total_alloc_size; /* Total size that needs to be allocated for new buffer (including existing one). */
314 uint16_t add_alloc_size; /* Additional space needed in case allocated memory needs to be increased. */
315 dm_free_header_t *entry_p;
316 uint32_t *next_addr, *new_buffer_p;
317 uint32_t *buffer32_p = buffer_p;
318
319 db_alloc_header_t *allocated_entry_p = (db_alloc_header_t *)(buffer32_p - 1);
320
321 total_alloc_size = ALIGN_UPTO_32BITS(size) + sizeof(db_alloc_header_t);
322
323 /* Check if current buffer has already the requested size.
324 If this is the case, try to reduce it. */
325 if(allocated_entry_p->buffer_size >= total_alloc_size)
326 {
327 allocated_entry_p->buffer_size = dm_slice((dm_free_header_t*)allocated_entry_p, total_alloc_size);
328
329 return buffer_p;
330 }
331
332 add_alloc_size = total_alloc_size - allocated_entry_p->buffer_size;
333
334 next_addr = &allocated_entry_p->buffer_a[(allocated_entry_p->buffer_size - sizeof(db_alloc_header_t)) >> 2];
335
336 entry_p = (dm_free_header_t*)next_addr;
337
338 /* Look into next block and check if it is free and has enough space to contain additional data. */
339 if(next_addr < dm_ctx.alloc_space_p + dm_ctx.buffer_size && entry_p->flags == DM_FREE)
340 {
341 if(entry_p->buffer_size >= add_alloc_size)
342 {
343 /* Next contiguous slot is big enough. */
344
345 db_extract_from_free_list(entry_p);
346
347 allocated_entry_p->buffer_size += dm_slice(entry_p, add_alloc_size);
348
349 #if defined(DM_DEBUG)
350 dm_ctx.alloc_size += alloc_size + sizeof(uint32_t);
351 if (dm_ctx.alloc_size > dm_ctx.alloc_max_size)
352 {
353 dm_ctx.alloc_max_size = dm_ctx.alloc_size;
354 }
355 #endif
356 return allocated_entry_p->buffer_a;
357 }
358 }
359
360 /* No contiguous free memory slot found with enough space.
361 Need to allocate new slot. */
362
363 new_buffer_p = dm_alloc(size);
364 if (new_buffer_p != NULL)
365 {
366 /* Copy old data */
367 uint16_t old_data_size = allocated_entry_p->buffer_size - sizeof(db_alloc_header_t);
368 Osal_MemCpy(new_buffer_p, buffer_p, MIN(size, old_data_size));
369 dm_free(buffer_p);
370 }
371
372 return new_buffer_p;
373 }
374
375
