Marco Zecchini
/
Example_RTOS
Rtos API example
Embed:
(wiki syntax)
Show/hide line numbers
nsdynmemLIB.c
00001 /* 00002 * Copyright (c) 2014-2015 ARM Limited. All rights reserved. 00003 * SPDX-License-Identifier: Apache-2.0 00004 * Licensed under the Apache License, Version 2.0 (the License); you may 00005 * not use this file except in compliance with the License. 00006 * You may obtain a copy of the License at 00007 * 00008 * http://www.apache.org/licenses/LICENSE-2.0 00009 * 00010 * Unless required by applicable law or agreed to in writing, software 00011 * distributed under the License is distributed on an AS IS BASIS, WITHOUT 00012 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00013 * See the License for the specific language governing permissions and 00014 * limitations under the License. 00015 */ 00016 #include <stdint.h> 00017 #include <string.h> 00018 #include "nsdynmemLIB.h" 00019 #include "platform/arm_hal_interrupt.h" 00020 #include <stdlib.h> 00021 #include "ns_list.h" 00022 00023 #ifndef STANDARD_MALLOC 00024 typedef enum mem_stat_update_t { 00025 DEV_HEAP_ALLOC_OK, 00026 DEV_HEAP_ALLOC_FAIL, 00027 DEV_HEAP_FREE, 00028 } mem_stat_update_t; 00029 00030 typedef struct { 00031 ns_list_link_t link; 00032 } hole_t; 00033 00034 typedef int ns_mem_word_size_t; // internal signed heap block size type 00035 00036 /* struct for book keeping variables */ 00037 struct ns_mem_book { 00038 ns_mem_word_size_t *heap_main; 00039 ns_mem_word_size_t *heap_main_end; 00040 mem_stat_t *mem_stat_info_ptr; 00041 void (*heap_failure_callback)(heap_fail_t); 00042 NS_LIST_HEAD(hole_t, link) holes_list; 00043 ns_mem_heap_size_t heap_size; 00044 }; 00045 00046 static ns_mem_book_t *default_book; // heap pointer for original "ns_" API use 00047 00048 // size of a hole_t in our word units 00049 #define HOLE_T_SIZE ((sizeof(hole_t) + sizeof(ns_mem_word_size_t) - 1) / sizeof(ns_mem_word_size_t)) 00050 00051 static NS_INLINE hole_t *hole_from_block_start(ns_mem_word_size_t *start) 00052 { 00053 return (hole_t *)(start + 1); 00054 } 00055 00056 static NS_INLINE ns_mem_word_size_t *block_start_from_hole(hole_t *start) 00057 { 00058 return ((ns_mem_word_size_t *)start) - 1; 00059 } 00060 00061 static void heap_failure(ns_mem_book_t *book, heap_fail_t reason) 00062 { 00063 if (book->heap_failure_callback) { 00064 book->heap_failure_callback(reason); 00065 } 00066 } 00067 00068 #endif 00069 00070 void ns_dyn_mem_init(void *heap, ns_mem_heap_size_t h_size, 00071 void (*passed_fptr)(heap_fail_t), mem_stat_t *info_ptr) 00072 { 00073 default_book = ns_mem_init(heap, h_size, passed_fptr, info_ptr); 00074 } 00075 00076 const mem_stat_t *ns_dyn_mem_get_mem_stat(void) 00077 { 00078 #ifndef STANDARD_MALLOC 00079 return ns_mem_get_mem_stat(default_book); 00080 #else 00081 return NULL; 00082 #endif 00083 } 00084 00085 00086 ns_mem_book_t *ns_mem_init(void *heap, ns_mem_heap_size_t h_size, 00087 void (*passed_fptr)(heap_fail_t), 00088 mem_stat_t *info_ptr) 00089 { 00090 #ifndef STANDARD_MALLOC 00091 ns_mem_book_t *book; 00092 00093 ns_mem_word_size_t *ptr; 00094 ns_mem_word_size_t temp_int; 00095 /* Do memory alignment */ 00096 temp_int = ((uintptr_t)heap % sizeof(ns_mem_word_size_t)); 00097 if (temp_int) { 00098 heap = (uint8_t *) heap + (sizeof(ns_mem_word_size_t) - temp_int); 00099 h_size -= (sizeof(ns_mem_word_size_t) - temp_int); 00100 } 00101 00102 /* Make correction for total length also */ 00103 temp_int = (h_size % sizeof(ns_mem_word_size_t)); 00104 if (temp_int) { 00105 h_size -= (sizeof(ns_mem_word_size_t) - temp_int); 00106 } 00107 book = heap; 00108 book->heap_main = (ns_mem_word_size_t *)&(book[1]); // SET Heap Pointer 00109 book->heap_size = h_size - sizeof(ns_mem_book_t); //Set Heap Size 00110 temp_int = (book->heap_size / sizeof(ns_mem_word_size_t)); 00111 temp_int -= 2; 00112 ptr = book->heap_main; 00113 *ptr = -(temp_int); 00114 ptr += (temp_int + 1); 00115 *ptr = -(temp_int); 00116 book->heap_main_end = ptr; 00117 00118 ns_list_init(&book->holes_list); 00119 ns_list_add_to_start(&book->holes_list, hole_from_block_start(book->heap_main)); 00120 00121 book->mem_stat_info_ptr = info_ptr; 00122 //RESET Memory by Hea Len 00123 if (info_ptr) { 00124 memset(book->mem_stat_info_ptr, 0, sizeof(mem_stat_t)); 00125 book->mem_stat_info_ptr->heap_sector_size = book->heap_size; 00126 } 00127 #endif 00128 //There really is no support to standard malloc in this library anymore 00129 book->heap_failure_callback = passed_fptr; 00130 00131 return book; 00132 } 00133 00134 const mem_stat_t *ns_mem_get_mem_stat(ns_mem_book_t *heap) 00135 { 00136 #ifndef STANDARD_MALLOC 00137 return heap->mem_stat_info_ptr; 00138 #else 00139 return NULL; 00140 #endif 00141 } 00142 00143 #ifndef STANDARD_MALLOC 00144 static void dev_stat_update(mem_stat_t *mem_stat_info_ptr, mem_stat_update_t type, ns_mem_block_size_t size) 00145 { 00146 if (mem_stat_info_ptr) { 00147 switch (type) { 00148 case DEV_HEAP_ALLOC_OK: 00149 mem_stat_info_ptr->heap_sector_alloc_cnt++; 00150 mem_stat_info_ptr->heap_sector_allocated_bytes += size; 00151 if (mem_stat_info_ptr->heap_sector_allocated_bytes_max < mem_stat_info_ptr->heap_sector_allocated_bytes) { 00152 mem_stat_info_ptr->heap_sector_allocated_bytes_max = mem_stat_info_ptr->heap_sector_allocated_bytes; 00153 } 00154 mem_stat_info_ptr->heap_alloc_total_bytes += size; 00155 break; 00156 case DEV_HEAP_ALLOC_FAIL: 00157 mem_stat_info_ptr->heap_alloc_fail_cnt++; 00158 break; 00159 case DEV_HEAP_FREE: 00160 mem_stat_info_ptr->heap_sector_alloc_cnt--; 00161 mem_stat_info_ptr->heap_sector_allocated_bytes -= size; 00162 break; 00163 } 00164 } 00165 } 00166 00167 static ns_mem_word_size_t convert_allocation_size(ns_mem_book_t *book, ns_mem_block_size_t requested_bytes) 00168 { 00169 if (book->heap_main == 0) { 00170 heap_failure(book, NS_DYN_MEM_HEAP_SECTOR_UNITIALIZED); 00171 } else if (requested_bytes < 1) { 00172 heap_failure(book, NS_DYN_MEM_ALLOCATE_SIZE_NOT_VALID); 00173 } else if (requested_bytes > (book->heap_size - 2 * sizeof(ns_mem_word_size_t)) ) { 00174 heap_failure(book, NS_DYN_MEM_ALLOCATE_SIZE_NOT_VALID); 00175 } 00176 return (requested_bytes + sizeof(ns_mem_word_size_t) - 1) / sizeof(ns_mem_word_size_t); 00177 } 00178 00179 // Checks that block length indicators are valid 00180 // Block has format: Size of data area [1 word] | data area [abs(size) words]| Size of data area [1 word] 00181 // If Size is negative it means area is unallocated 00182 // For direction, use 1 for direction up and -1 for down 00183 static int8_t ns_mem_block_validate(ns_mem_word_size_t *block_start, int direction) 00184 { 00185 int8_t ret_val = -1; 00186 ns_mem_word_size_t *end = block_start; 00187 ns_mem_word_size_t size_start = *end; 00188 end += (1 + abs(size_start)); 00189 if (size_start != 0 && size_start == *end) { 00190 ret_val = 0; 00191 } 00192 return ret_val; 00193 } 00194 #endif 00195 00196 // For direction, use 1 for direction up and -1 for down 00197 static void *ns_mem_internal_alloc(ns_mem_book_t *book, const ns_mem_block_size_t alloc_size, int direction) 00198 { 00199 #ifndef STANDARD_MALLOC 00200 if (!book) { 00201 /* We can not do anything except return NULL because we can't find book 00202 keeping block */ 00203 return NULL; 00204 } 00205 00206 ns_mem_word_size_t *block_ptr = NULL; 00207 00208 platform_enter_critical(); 00209 00210 ns_mem_word_size_t data_size = convert_allocation_size(book, alloc_size); 00211 if (!data_size) { 00212 goto done; 00213 } 00214 00215 // ns_list_foreach, either forwards or backwards, result to ptr 00216 for (hole_t *cur_hole = direction > 0 ? ns_list_get_first(&book->holes_list) 00217 : ns_list_get_last(&book->holes_list); 00218 cur_hole; 00219 cur_hole = direction > 0 ? ns_list_get_next(&book->holes_list, cur_hole) 00220 : ns_list_get_previous(&book->holes_list, cur_hole) 00221 ) { 00222 ns_mem_word_size_t *p = block_start_from_hole(cur_hole); 00223 if (ns_mem_block_validate(p, direction) != 0 || *p >= 0) { 00224 //Validation failed, or this supposed hole has positive (allocated) size 00225 heap_failure(book, NS_DYN_MEM_HEAP_SECTOR_CORRUPTED); 00226 break; 00227 } 00228 if (-*p >= data_size) { 00229 // Found a big enough block 00230 block_ptr = p; 00231 break; 00232 } 00233 } 00234 00235 if (!block_ptr) { 00236 goto done; 00237 } 00238 00239 ns_mem_word_size_t block_data_size = -*block_ptr; 00240 if (block_data_size >= (data_size + 2 + HOLE_T_SIZE)) { 00241 ns_mem_word_size_t hole_size = block_data_size - data_size - 2; 00242 ns_mem_word_size_t *hole_ptr; 00243 //There is enough room for a new hole so create it first 00244 if ( direction > 0 ) { 00245 hole_ptr = block_ptr + 1 + data_size + 1; 00246 // Hole will be left at end of area. 00247 // Would like to just replace this block_ptr with new descriptor, but 00248 // they could overlap, so ns_list_replace might fail 00249 //ns_list_replace(&holes_list, block_ptr, hole_from_block_start(hole_ptr)); 00250 hole_t *before = ns_list_get_previous(&book->holes_list, hole_from_block_start(block_ptr)); 00251 ns_list_remove(&book->holes_list, hole_from_block_start(block_ptr)); 00252 if (before) { 00253 ns_list_add_after(&book->holes_list, before, hole_from_block_start(hole_ptr)); 00254 } else { 00255 ns_list_add_to_start(&book->holes_list, hole_from_block_start(hole_ptr)); 00256 } 00257 } else { 00258 hole_ptr = block_ptr; 00259 // Hole remains at start of area - keep existing descriptor in place. 00260 block_ptr += 1 + hole_size + 1; 00261 } 00262 00263 hole_ptr[0] = -hole_size; 00264 hole_ptr[1 + hole_size] = -hole_size; 00265 } else { 00266 // Not enough room for a left-over hole, so use the whole block 00267 data_size = block_data_size; 00268 ns_list_remove(&book->holes_list, hole_from_block_start(block_ptr)); 00269 } 00270 block_ptr[0] = data_size; 00271 block_ptr[1 + data_size] = data_size; 00272 00273 done: 00274 if (book->mem_stat_info_ptr) { 00275 if (block_ptr) { 00276 //Update Allocate OK 00277 dev_stat_update(book->mem_stat_info_ptr, DEV_HEAP_ALLOC_OK, (data_size + 2) * sizeof(ns_mem_word_size_t)); 00278 00279 } else { 00280 //Update Allocate Fail, second parameter is not used for stats 00281 dev_stat_update(book->mem_stat_info_ptr, DEV_HEAP_ALLOC_FAIL, 0); 00282 } 00283 } 00284 platform_exit_critical(); 00285 00286 return block_ptr ? block_ptr + 1 : NULL; 00287 #else 00288 void *retval = NULL; 00289 if (alloc_size) { 00290 platform_enter_critical(); 00291 retval = malloc(alloc_size); 00292 platform_exit_critical(); 00293 } 00294 return retval; 00295 #endif 00296 } 00297 00298 void *ns_mem_alloc(ns_mem_book_t *heap, ns_mem_block_size_t alloc_size) 00299 { 00300 return ns_mem_internal_alloc(heap, alloc_size, -1); 00301 } 00302 00303 void *ns_mem_temporary_alloc(ns_mem_book_t *heap, ns_mem_block_size_t alloc_size) 00304 { 00305 return ns_mem_internal_alloc(heap, alloc_size, 1); 00306 } 00307 00308 void *ns_dyn_mem_alloc(ns_mem_block_size_t alloc_size) 00309 { 00310 return ns_mem_alloc(default_book, alloc_size); 00311 } 00312 00313 void *ns_dyn_mem_temporary_alloc(ns_mem_block_size_t alloc_size) 00314 { 00315 return ns_mem_temporary_alloc(default_book, alloc_size); 00316 } 00317 00318 #ifndef STANDARD_MALLOC 00319 static void ns_mem_free_and_merge_with_adjacent_blocks(ns_mem_book_t *book, ns_mem_word_size_t *cur_block, ns_mem_word_size_t data_size) 00320 { 00321 // Theory of operation: Block is always in form | Len | Data | Len | 00322 // So we need to check length of previous (if current not heap start) 00323 // and next (if current not heap end) blocks. Negative length means 00324 // free memory so we can merge freed block with those. 00325 00326 hole_t *existing_start = NULL; 00327 hole_t *existing_end = NULL; 00328 ns_mem_word_size_t *start = cur_block; 00329 ns_mem_word_size_t *end = cur_block + data_size + 1; 00330 //invalidate current block 00331 *start = -data_size; 00332 *end = -data_size; 00333 ns_mem_word_size_t merged_data_size = data_size; 00334 00335 if (start != book->heap_main) { 00336 if (*(start - 1) < 0) { 00337 int *block_end = start - 1; 00338 size_t block_size = 1 + (-*block_end) + 1; 00339 merged_data_size += block_size; 00340 start -= block_size; 00341 if (*start != *block_end) { 00342 heap_failure(book, NS_DYN_MEM_HEAP_SECTOR_CORRUPTED); 00343 } 00344 if (block_size >= 1 + HOLE_T_SIZE + 1) { 00345 existing_start = hole_from_block_start(start); 00346 } 00347 } 00348 } 00349 00350 if (end != book->heap_main_end) { 00351 if (*(end + 1) < 0) { 00352 int *block_start = end + 1; 00353 size_t block_size = 1 + (-*block_start) + 1; 00354 merged_data_size += block_size; 00355 end += block_size; 00356 if (*end != *block_start) { 00357 heap_failure(book, NS_DYN_MEM_HEAP_SECTOR_CORRUPTED); 00358 } 00359 if (block_size >= 1 + HOLE_T_SIZE + 1) { 00360 existing_end = hole_from_block_start(block_start); 00361 } 00362 } 00363 } 00364 00365 hole_t *to_add = hole_from_block_start(start); 00366 hole_t *before = NULL; 00367 if (existing_end) { 00368 // Extending hole described by "existing_end" downwards. 00369 // Will replace with descriptor at bottom of merged block. 00370 // (Can't use ns_list_replace, because of danger of overlap) 00371 // Optimisation - note our position for insertion below. 00372 before = ns_list_get_next(&book->holes_list, existing_end); 00373 ns_list_remove(&book->holes_list, existing_end); 00374 } 00375 if (existing_start) { 00376 // Extending hole described by "existing_start" upwards. 00377 // No need to modify that descriptor - it remains at the bottom 00378 // of the merged block to describe it. 00379 } else { 00380 // Didn't find adjacent descriptors, but may still 00381 // be merging with small blocks without descriptors. 00382 if ( merged_data_size >= HOLE_T_SIZE ) { 00383 // Locate hole position in list, if we don't already know 00384 // from merging with the block above. 00385 if (!existing_end) { 00386 ns_list_foreach(hole_t, ptr, &book->holes_list) { 00387 if (ptr > to_add) { 00388 before = ptr; 00389 break; 00390 } 00391 } 00392 } 00393 if (before) { 00394 ns_list_add_before(&book->holes_list, before, to_add); 00395 } else { 00396 ns_list_add_to_end(&book->holes_list, to_add); 00397 } 00398 00399 } 00400 } 00401 *start = -merged_data_size; 00402 *end = -merged_data_size; 00403 } 00404 #endif 00405 00406 void ns_mem_free(ns_mem_book_t *book, void *block) 00407 { 00408 #ifndef STANDARD_MALLOC 00409 00410 if (!block) { 00411 return; 00412 } 00413 00414 ns_mem_word_size_t *ptr = block; 00415 ns_mem_word_size_t size; 00416 00417 platform_enter_critical(); 00418 ptr --; 00419 //Read Current Size 00420 size = *ptr; 00421 if (ptr < book->heap_main || ptr >= book->heap_main_end) { 00422 heap_failure(book, NS_DYN_MEM_POINTER_NOT_VALID); 00423 } else if ((ptr + size) >= book->heap_main_end) { 00424 heap_failure(book, NS_DYN_MEM_POINTER_NOT_VALID); 00425 } else if (size < 0) { 00426 heap_failure(book, NS_DYN_MEM_DOUBLE_FREE); 00427 } else { 00428 if (ns_mem_block_validate(ptr, 1) != 0) { 00429 heap_failure(book, NS_DYN_MEM_HEAP_SECTOR_CORRUPTED); 00430 } else { 00431 ns_mem_free_and_merge_with_adjacent_blocks(book, ptr, size); 00432 if (book->mem_stat_info_ptr) { 00433 //Update Free Counter 00434 dev_stat_update(book->mem_stat_info_ptr, DEV_HEAP_FREE, (size + 2) * sizeof(ns_mem_word_size_t)); 00435 } 00436 } 00437 } 00438 platform_exit_critical(); 00439 #else 00440 platform_enter_critical(); 00441 free(block); 00442 platform_exit_critical(); 00443 #endif 00444 } 00445 00446 void ns_dyn_mem_free(void *block) 00447 { 00448 ns_mem_free(default_book, block); 00449 }
Generated on Sun Jul 17 2022 08:25:28 by 1.7.2