takashi kadono / Mbed OS Nucleo_446

Dependencies:   ssd1331

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers nsdynmemLIB.c Source File

nsdynmemLIB.c

00001 /*
00002  * Copyright (c) 2014-2018 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     ns_mem_heap_size_t temporary_alloc_heap_limit;   /* Amount of reserved heap temporary alloc can't exceed */
00045 };
00046 
00047 static ns_mem_book_t *default_book; // heap pointer for original "ns_" API use
00048 
00049 // size of a hole_t in our word units
00050 #define HOLE_T_SIZE ((ns_mem_word_size_t) ((sizeof(hole_t) + sizeof(ns_mem_word_size_t) - 1) / sizeof(ns_mem_word_size_t)))
00051 
00052 #define TEMPORARY_ALLOC_FREE_HEAP_THRESHOLD 5  /* temporary allocations must leave 5% of the heap free */
00053 
00054 static NS_INLINE hole_t *hole_from_block_start(ns_mem_word_size_t *start)
00055 {
00056     return (hole_t *)(start + 1);
00057 }
00058 
00059 static NS_INLINE ns_mem_word_size_t *block_start_from_hole(hole_t *start)
00060 {
00061     return ((ns_mem_word_size_t *)start) - 1;
00062 }
00063 
00064 static void heap_failure(ns_mem_book_t *book, heap_fail_t reason)
00065 {
00066     if (book->heap_failure_callback) {
00067         book->heap_failure_callback(reason);
00068     }
00069 }
00070 
00071 #endif
00072 
00073 void ns_dyn_mem_init(void *heap, ns_mem_heap_size_t h_size,
00074                      void (*passed_fptr)(heap_fail_t), mem_stat_t *info_ptr)
00075 {
00076     default_book = ns_mem_init(heap, h_size, passed_fptr, info_ptr);
00077 }
00078 
00079 const mem_stat_t *ns_dyn_mem_get_mem_stat(void)
00080 {
00081 #ifndef STANDARD_MALLOC
00082     return ns_mem_get_mem_stat(default_book);
00083 #else
00084     return NULL;
00085 #endif
00086 }
00087 
00088 
00089 ns_mem_book_t *ns_mem_init(void *heap, ns_mem_heap_size_t h_size,
00090                          void (*passed_fptr)(heap_fail_t),
00091                                       mem_stat_t *info_ptr)
00092 {
00093 #ifndef STANDARD_MALLOC
00094     ns_mem_book_t *book;
00095 
00096     ns_mem_word_size_t *ptr;
00097     ns_mem_word_size_t temp_int;
00098     /* Do memory alignment */
00099     temp_int = ((uintptr_t)heap % sizeof(ns_mem_word_size_t));
00100     if (temp_int) {
00101         heap = (uint8_t *) heap + (sizeof(ns_mem_word_size_t) - temp_int);
00102         h_size -= (sizeof(ns_mem_word_size_t) - temp_int);
00103     }
00104 
00105     /* Make correction for total length also */
00106     temp_int = (h_size % sizeof(ns_mem_word_size_t));
00107     if (temp_int) {
00108         h_size -= (sizeof(ns_mem_word_size_t) - temp_int);
00109     }
00110     book = heap;
00111     book->heap_main = (ns_mem_word_size_t *)&(book[1]); // SET Heap Pointer
00112     book->heap_size = h_size - sizeof(ns_mem_book_t); //Set Heap Size
00113     temp_int = (book->heap_size / sizeof(ns_mem_word_size_t));
00114     temp_int -= 2;
00115     ptr = book->heap_main;
00116     *ptr = -(temp_int);
00117     ptr += (temp_int + 1);
00118     *ptr = -(temp_int);
00119     book->heap_main_end = ptr;
00120 
00121     ns_list_init(&book->holes_list);
00122     ns_list_add_to_start(&book->holes_list, hole_from_block_start(book->heap_main));
00123 
00124     book->mem_stat_info_ptr = info_ptr;
00125     //RESET Memory by Hea Len
00126     if (info_ptr) {
00127         memset(book->mem_stat_info_ptr, 0, sizeof(mem_stat_t));
00128         book->mem_stat_info_ptr->heap_sector_size = book->heap_size;
00129     }
00130     book->temporary_alloc_heap_limit = book->heap_size/100 * (100-TEMPORARY_ALLOC_FREE_HEAP_THRESHOLD);
00131 #endif
00132     //There really is no support to standard malloc in this library anymore
00133     book->heap_failure_callback = passed_fptr;
00134 
00135     return book;
00136 }
00137 
00138 const mem_stat_t *ns_mem_get_mem_stat(ns_mem_book_t *heap)
00139 {
00140 #ifndef STANDARD_MALLOC
00141     return heap->mem_stat_info_ptr;
00142 #else
00143     return NULL;
00144 #endif
00145 }
00146 
00147 int ns_mem_set_temporary_alloc_free_heap_threshold(ns_mem_book_t *book, uint8_t free_heap_percentage, ns_mem_heap_size_t free_heap_amount)
00148 {
00149 #ifndef STANDARD_MALLOC
00150     ns_mem_heap_size_t heap_limit = 0;
00151 
00152     if (!book || !book->mem_stat_info_ptr) {
00153         // no book or mem_stats
00154         return -1;
00155     }
00156 
00157     if (free_heap_amount && free_heap_amount < book->heap_size/2) {
00158         heap_limit = book->heap_size - free_heap_amount;
00159     }
00160 
00161     if (!free_heap_amount && free_heap_percentage && free_heap_percentage < 50) {
00162         heap_limit = book->heap_size/100 * (100 - free_heap_percentage);
00163     }
00164 
00165     if (free_heap_amount == 0 && free_heap_percentage == 0) {
00166         // feature disabled, allow whole heap to be reserved by temporary allo
00167         heap_limit = book->heap_size;
00168     }
00169 
00170     if (heap_limit == 0) {
00171         // illegal heap parameters
00172         return -2;
00173     }
00174 
00175     book->temporary_alloc_heap_limit = heap_limit;
00176 
00177     return 0;
00178 #else
00179     return -3;
00180 #endif
00181 }
00182 
00183 extern int ns_dyn_mem_set_temporary_alloc_free_heap_threshold(uint8_t free_heap_percentage, ns_mem_heap_size_t free_heap_amount)
00184 {
00185     return ns_mem_set_temporary_alloc_free_heap_threshold(default_book, free_heap_percentage, free_heap_amount);
00186 }
00187 
00188 #ifndef STANDARD_MALLOC
00189 static void dev_stat_update(mem_stat_t *mem_stat_info_ptr, mem_stat_update_t type, ns_mem_block_size_t size)
00190 {
00191     if (mem_stat_info_ptr) {
00192         switch (type) {
00193             case DEV_HEAP_ALLOC_OK:
00194                 mem_stat_info_ptr->heap_sector_alloc_cnt++;
00195                 mem_stat_info_ptr->heap_sector_allocated_bytes += size;
00196                 if (mem_stat_info_ptr->heap_sector_allocated_bytes_max < mem_stat_info_ptr->heap_sector_allocated_bytes) {
00197                     mem_stat_info_ptr->heap_sector_allocated_bytes_max = mem_stat_info_ptr->heap_sector_allocated_bytes;
00198                 }
00199                 mem_stat_info_ptr->heap_alloc_total_bytes += size;
00200                 break;
00201             case DEV_HEAP_ALLOC_FAIL:
00202                 mem_stat_info_ptr->heap_alloc_fail_cnt++;
00203                 break;
00204             case DEV_HEAP_FREE:
00205                 mem_stat_info_ptr->heap_sector_alloc_cnt--;
00206                 mem_stat_info_ptr->heap_sector_allocated_bytes -= size;
00207                 break;
00208         }
00209     }
00210 }
00211 
00212 static ns_mem_word_size_t convert_allocation_size(ns_mem_book_t *book, ns_mem_block_size_t requested_bytes)
00213 {
00214     if (book->heap_main == 0) {
00215         heap_failure(book, NS_DYN_MEM_HEAP_SECTOR_UNITIALIZED);
00216     } else if (requested_bytes < 1) {
00217         heap_failure(book, NS_DYN_MEM_ALLOCATE_SIZE_NOT_VALID);
00218     } else if (requested_bytes > (book->heap_size - 2 * sizeof(ns_mem_word_size_t)) ) {
00219         heap_failure(book, NS_DYN_MEM_ALLOCATE_SIZE_NOT_VALID);
00220     }
00221     return (requested_bytes + sizeof(ns_mem_word_size_t) - 1) / sizeof(ns_mem_word_size_t);
00222 }
00223 
00224 // Checks that block length indicators are valid
00225 // Block has format: Size of data area [1 word] | data area [abs(size) words]| Size of data area [1 word]
00226 // If Size is negative it means area is unallocated
00227 static int8_t ns_mem_block_validate(ns_mem_word_size_t *block_start)
00228 {
00229     int8_t ret_val = -1;
00230     ns_mem_word_size_t *end = block_start;
00231     ns_mem_word_size_t size_start = *end;
00232     end += (1 + abs(size_start));
00233     if (size_start != 0 && size_start == *end) {
00234         ret_val = 0;
00235     }
00236     return ret_val;
00237 }
00238 #endif
00239 
00240 // For direction, use 1 for direction up and -1 for down
00241 static void *ns_mem_internal_alloc(ns_mem_book_t *book, const ns_mem_block_size_t alloc_size, int direction)
00242 {
00243 #ifndef STANDARD_MALLOC
00244     if (!book) {
00245         /* We can not do anything except return NULL because we can't find book
00246            keeping block */
00247         return NULL;
00248     }
00249 
00250     if (book->mem_stat_info_ptr && direction == 1) {
00251         if (book->mem_stat_info_ptr->heap_sector_allocated_bytes > book->temporary_alloc_heap_limit) {
00252             /* Not enough heap for temporary memory allocation */
00253             dev_stat_update(book->mem_stat_info_ptr, DEV_HEAP_ALLOC_FAIL, 0);
00254             return NULL;
00255         }
00256     }
00257 
00258     ns_mem_word_size_t *block_ptr = NULL;
00259 
00260     platform_enter_critical();
00261 
00262     ns_mem_word_size_t data_size = convert_allocation_size(book, alloc_size);
00263     if (!data_size) {
00264         goto done;
00265     }
00266 
00267     // ns_list_foreach, either forwards or backwards, result to ptr
00268     for (hole_t *cur_hole = direction > 0 ? ns_list_get_first(&book->holes_list)
00269                                           : ns_list_get_last(&book->holes_list);
00270          cur_hole;
00271          cur_hole = direction > 0 ? ns_list_get_next(&book->holes_list, cur_hole)
00272                                   : ns_list_get_previous(&book->holes_list, cur_hole)
00273         ) {
00274         ns_mem_word_size_t *p = block_start_from_hole(cur_hole);
00275         if (ns_mem_block_validate(p) != 0 || *p >= 0) {
00276             //Validation failed, or this supposed hole has positive (allocated) size
00277             heap_failure(book, NS_DYN_MEM_HEAP_SECTOR_CORRUPTED);
00278             break;
00279         }
00280         if (-*p >= data_size) {
00281             // Found a big enough block
00282             block_ptr = p;
00283             break;
00284         }
00285     }
00286 
00287     if (!block_ptr) {
00288         goto done;
00289     }
00290 
00291     // Separate declaration from initialization to keep IAR happy as the gotos skip this block.
00292     ns_mem_word_size_t block_data_size;
00293     block_data_size = -*block_ptr;
00294     if (block_data_size >= (data_size + 2 + HOLE_T_SIZE)) {
00295         ns_mem_word_size_t hole_size = block_data_size - data_size - 2;
00296         ns_mem_word_size_t *hole_ptr;
00297         //There is enough room for a new hole so create it first
00298         if ( direction > 0 ) {
00299             hole_ptr = block_ptr + 1 + data_size + 1;
00300             // Hole will be left at end of area.
00301             // Would like to just replace this block_ptr with new descriptor, but
00302             // they could overlap, so ns_list_replace might fail
00303             //ns_list_replace(&holes_list, block_ptr, hole_from_block_start(hole_ptr));
00304             hole_t *before = ns_list_get_previous(&book->holes_list, hole_from_block_start(block_ptr));
00305             ns_list_remove(&book->holes_list, hole_from_block_start(block_ptr));
00306             if (before) {
00307                 ns_list_add_after(&book->holes_list, before, hole_from_block_start(hole_ptr));
00308             } else {
00309                 ns_list_add_to_start(&book->holes_list, hole_from_block_start(hole_ptr));
00310             }
00311         } else {
00312             hole_ptr = block_ptr;
00313             // Hole remains at start of area - keep existing descriptor in place.
00314             block_ptr += 1 + hole_size + 1;
00315         }
00316 
00317         hole_ptr[0] = -hole_size;
00318         hole_ptr[1 + hole_size] = -hole_size;
00319     } else {
00320         // Not enough room for a left-over hole, so use the whole block
00321         data_size = block_data_size;
00322         ns_list_remove(&book->holes_list, hole_from_block_start(block_ptr));
00323     }
00324     block_ptr[0] = data_size;
00325     block_ptr[1 + data_size] = data_size;
00326 
00327  done:
00328     if (book->mem_stat_info_ptr) {
00329         if (block_ptr) {
00330             //Update Allocate OK
00331             dev_stat_update(book->mem_stat_info_ptr, DEV_HEAP_ALLOC_OK, (data_size + 2) * sizeof(ns_mem_word_size_t));
00332 
00333         } else {
00334             //Update Allocate Fail, second parameter is used for stats
00335             dev_stat_update(book->mem_stat_info_ptr, DEV_HEAP_ALLOC_FAIL, 0);
00336         }
00337     }
00338     platform_exit_critical();
00339 
00340     return block_ptr ? block_ptr + 1 : NULL;
00341 #else
00342     void *retval = NULL;
00343     if (alloc_size) {
00344         platform_enter_critical();
00345         retval = malloc(alloc_size);
00346         platform_exit_critical();
00347     }
00348     return retval;
00349 #endif
00350 }
00351 
00352 void *ns_mem_alloc(ns_mem_book_t *heap, ns_mem_block_size_t alloc_size)
00353 {
00354     return ns_mem_internal_alloc(heap, alloc_size, -1);
00355 }
00356 
00357 void *ns_mem_temporary_alloc(ns_mem_book_t *heap, ns_mem_block_size_t alloc_size)
00358 {
00359     return ns_mem_internal_alloc(heap, alloc_size, 1);
00360 }
00361 
00362 void *ns_dyn_mem_alloc(ns_mem_block_size_t alloc_size)
00363 {
00364     return ns_mem_alloc(default_book, alloc_size);
00365 }
00366 
00367 void *ns_dyn_mem_temporary_alloc(ns_mem_block_size_t alloc_size)
00368 {
00369     return ns_mem_temporary_alloc(default_book, alloc_size);
00370 }
00371 
00372 #ifndef STANDARD_MALLOC
00373 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)
00374 {
00375     // Theory of operation: Block is always in form | Len | Data | Len |
00376     // So we need to check length of previous (if current not heap start)
00377     // and next (if current not heap end) blocks. Negative length means
00378     // free memory so we can merge freed block with those.
00379 
00380     hole_t *existing_start = NULL;
00381     hole_t *existing_end = NULL;
00382     ns_mem_word_size_t *start = cur_block;
00383     ns_mem_word_size_t *end = cur_block + data_size + 1;
00384     //invalidate current block
00385     *start = -data_size;
00386     *end = -data_size;
00387     ns_mem_word_size_t merged_data_size = data_size;
00388 
00389     if (start != book->heap_main) {
00390         if (*(start - 1) < 0) {
00391             ns_mem_word_size_t *block_end = start - 1;
00392             ns_mem_word_size_t block_size = 1 + (-*block_end) + 1;
00393             merged_data_size += block_size;
00394             start -= block_size;
00395             if (*start != *block_end) {
00396                 heap_failure(book, NS_DYN_MEM_HEAP_SECTOR_CORRUPTED);
00397             }
00398             if (block_size >= 1 + HOLE_T_SIZE + 1) {
00399                 existing_start = hole_from_block_start(start);
00400             }
00401         }
00402     }
00403 
00404     if (end != book->heap_main_end) {
00405         if (*(end + 1) < 0) {
00406             ns_mem_word_size_t *block_start = end + 1;
00407             ns_mem_word_size_t block_size = 1 + (-*block_start) + 1;
00408             merged_data_size += block_size;
00409             end += block_size;
00410             if (*end != *block_start) {
00411                 heap_failure(book, NS_DYN_MEM_HEAP_SECTOR_CORRUPTED);
00412             }
00413             if (block_size >= 1 + HOLE_T_SIZE + 1) {
00414                 existing_end = hole_from_block_start(block_start);
00415             }
00416         }
00417     }
00418 
00419     hole_t *to_add = hole_from_block_start(start);
00420     hole_t *before = NULL;
00421     if (existing_end) {
00422         // Extending hole described by "existing_end" downwards.
00423         // Will replace with descriptor at bottom of merged block.
00424         // (Can't use ns_list_replace, because of danger of overlap)
00425         // Optimisation - note our position for insertion below.
00426         before = ns_list_get_next(&book->holes_list, existing_end);
00427         ns_list_remove(&book->holes_list, existing_end);
00428     }
00429     if (existing_start) {
00430         // Extending hole described by "existing_start" upwards.
00431         // No need to modify that descriptor - it remains at the bottom
00432         // of the merged block to describe it.
00433     } else {
00434         // Didn't find adjacent descriptors, but may still
00435         // be merging with small blocks without descriptors.
00436         if ( merged_data_size >= HOLE_T_SIZE ) {
00437             // Locate hole position in list, if we don't already know
00438             // from merging with the block above.
00439             if (!existing_end) {
00440                 ns_list_foreach(hole_t, ptr, &book->holes_list) {
00441                     if (ptr > to_add) {
00442                         before = ptr;
00443                         break;
00444                     }
00445                 }
00446             }
00447             if (before) {
00448                 ns_list_add_before(&book->holes_list, before, to_add);
00449             } else {
00450                 ns_list_add_to_end(&book->holes_list, to_add);
00451             }
00452 
00453         }
00454     }
00455     *start = -merged_data_size;
00456     *end = -merged_data_size;
00457 }
00458 #endif
00459 
00460 void ns_mem_free(ns_mem_book_t *book, void *block)
00461 {
00462 #ifndef STANDARD_MALLOC
00463 
00464     if (!block) {
00465         return;
00466     }
00467 
00468     ns_mem_word_size_t *ptr = block;
00469     ns_mem_word_size_t size;
00470 
00471     platform_enter_critical();
00472     ptr --;
00473     //Read Current Size
00474     size = *ptr;
00475     if (ptr < book->heap_main || ptr >= book->heap_main_end) {
00476         heap_failure(book, NS_DYN_MEM_POINTER_NOT_VALID);
00477     } else if ((ptr + size) >= book->heap_main_end) {
00478         heap_failure(book, NS_DYN_MEM_POINTER_NOT_VALID);
00479     } else if (size < 0) {
00480         heap_failure(book, NS_DYN_MEM_DOUBLE_FREE);
00481     } else {
00482         if (ns_mem_block_validate(ptr) != 0) {
00483             heap_failure(book, NS_DYN_MEM_HEAP_SECTOR_CORRUPTED);
00484         } else {
00485             ns_mem_free_and_merge_with_adjacent_blocks(book, ptr, size);
00486             if (book->mem_stat_info_ptr) {
00487                 //Update Free Counter
00488                 dev_stat_update(book->mem_stat_info_ptr, DEV_HEAP_FREE, (size + 2) * sizeof(ns_mem_word_size_t));
00489             }
00490         }
00491     }
00492     platform_exit_critical();
00493 #else
00494     platform_enter_critical();
00495     free(block);
00496     platform_exit_critical();
00497 #endif
00498 }
00499 
00500 void ns_dyn_mem_free(void *block)
00501 {
00502     ns_mem_free(default_book, block);
00503 }