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 void (*heap_failure_callback)(heap_fail_t);
00024 
00025 #ifndef STANDARD_MALLOC
00026 static int *heap_main = 0;
00027 static int *heap_main_end = 0;
00028 static uint16_t heap_size = 0;
00029 
00030 typedef enum mem_stat_update_t {
00031     DEV_HEAP_ALLOC_OK,
00032     DEV_HEAP_ALLOC_FAIL,
00033     DEV_HEAP_FREE,
00034 } mem_stat_update_t;
00035 
00036 
00037 static mem_stat_t *mem_stat_info_ptr = 0;
00038 
00039 typedef struct {
00040     ns_list_link_t link;
00041 } hole_t;
00042 
00043 static NS_LIST_DEFINE(holes_list, hole_t, link);
00044 
00045 // size of a hole_t in our word units
00046 #define HOLE_T_SIZE ((sizeof(hole_t) + sizeof(int) - 1) / sizeof(int))
00047 
00048 static NS_INLINE hole_t *hole_from_block_start(int *start)
00049 {
00050     return (hole_t *)(start + 1);
00051 }
00052 
00053 static NS_INLINE int *block_start_from_hole(hole_t *start)
00054 {
00055     return ((int *)start) - 1;
00056 }
00057 
00058 
00059 static void heap_failure(heap_fail_t reason)
00060 {
00061     if (heap_failure_callback) {
00062         heap_failure_callback(reason);
00063     }
00064 }
00065 
00066 #endif
00067 
00068 void ns_dyn_mem_init(uint8_t *heap, uint16_t h_size, void (*passed_fptr)(heap_fail_t), mem_stat_t *info_ptr)
00069 {
00070 #ifndef STANDARD_MALLOC
00071     int *ptr;
00072     int temp_int;
00073     /* Do memory alignment */
00074     temp_int = ((uintptr_t)heap % sizeof(int));
00075     if (temp_int) {
00076         heap += (sizeof(int) - temp_int);
00077         h_size -= (sizeof(int) - temp_int);
00078     }
00079 
00080     /* Make correction for total length also */
00081     temp_int = (h_size % sizeof(int));
00082     if (temp_int) {
00083         h_size -= (sizeof(int) - temp_int);
00084     }
00085     heap_main = (int *)heap; // SET Heap Pointer
00086     heap_size = h_size; //Set Heap Size
00087     temp_int = (h_size / sizeof(int));
00088     temp_int -= 2;
00089     ptr = heap_main;
00090     *ptr = -(temp_int);
00091     ptr += (temp_int + 1);
00092     *ptr = -(temp_int);
00093     heap_main_end = ptr;
00094 
00095     ns_list_init(&holes_list);
00096     ns_list_add_to_start(&holes_list, hole_from_block_start(heap_main));
00097 
00098     //RESET Memory by Hea Len
00099     if (info_ptr) {
00100         mem_stat_info_ptr = info_ptr;
00101         memset(mem_stat_info_ptr, 0, sizeof(mem_stat_t));
00102         mem_stat_info_ptr->heap_sector_size = heap_size;
00103     }
00104 #endif
00105     heap_failure_callback = passed_fptr;
00106 }
00107 
00108 const mem_stat_t *ns_dyn_mem_get_mem_stat(void)
00109 {
00110 #ifndef STANDARD_MALLOC
00111     return mem_stat_info_ptr;
00112 #else
00113     return NULL;
00114 #endif
00115 }
00116 
00117 #ifndef STANDARD_MALLOC
00118 void dev_stat_update(mem_stat_update_t type, int16_t size)
00119 {
00120     if (mem_stat_info_ptr) {
00121         switch (type) {
00122             case DEV_HEAP_ALLOC_OK:
00123                 mem_stat_info_ptr->heap_sector_alloc_cnt++;
00124                 mem_stat_info_ptr->heap_sector_allocated_bytes += size;
00125                 if (mem_stat_info_ptr->heap_sector_allocated_bytes_max < mem_stat_info_ptr->heap_sector_allocated_bytes) {
00126                     mem_stat_info_ptr->heap_sector_allocated_bytes_max = mem_stat_info_ptr->heap_sector_allocated_bytes;
00127                 }
00128                 mem_stat_info_ptr->heap_alloc_total_bytes += size;
00129                 break;
00130             case DEV_HEAP_ALLOC_FAIL:
00131                 mem_stat_info_ptr->heap_alloc_fail_cnt++;
00132                 break;
00133             case DEV_HEAP_FREE:
00134                 mem_stat_info_ptr->heap_sector_alloc_cnt--;
00135                 mem_stat_info_ptr->heap_sector_allocated_bytes -= size;
00136                 break;
00137         }
00138     }
00139 }
00140 
00141 static int convert_allocation_size(int16_t requested_bytes)
00142 {
00143     if (heap_main == 0) {
00144         heap_failure(NS_DYN_MEM_HEAP_SECTOR_UNITIALIZED);
00145     } else if (requested_bytes < 1) {
00146         heap_failure(NS_DYN_MEM_ALLOCATE_SIZE_NOT_VALID);
00147     } else if (requested_bytes > (heap_size - 2 * sizeof(int)) ) {
00148         heap_failure(NS_DYN_MEM_ALLOCATE_SIZE_NOT_VALID);
00149     }
00150     return (requested_bytes + sizeof(int) - 1) / sizeof(int);
00151 }
00152 
00153 // Checks that block length indicators are valid
00154 // Block has format: Size of data area [1 word] | data area [abs(size) words]| Size of data area [1 word]
00155 // If Size is negative it means area is unallocated
00156 // For direction, use 1 for direction up and -1 for down
00157 static int8_t ns_block_validate(int *block_start, int direction)
00158 {
00159     int8_t ret_val = -1;
00160     int *end = block_start;
00161     int size_start = *end;
00162     end += (1 + abs(size_start));
00163     if (size_start != 0 && size_start == *end) {
00164         ret_val = 0;
00165     }
00166     return ret_val;
00167 }
00168 #endif
00169 
00170 // For direction, use 1 for direction up and -1 for down
00171 static void *ns_dyn_mem_internal_alloc(const int16_t alloc_size, int direction)
00172 {
00173 #ifndef STANDARD_MALLOC
00174     int *block_ptr = NULL;
00175 
00176     platform_enter_critical();
00177 
00178     int data_size = convert_allocation_size(alloc_size);
00179     if (!data_size) {
00180         goto done;
00181     }
00182 
00183     // ns_list_foreach, either forwards or backwards, result to ptr
00184     for (hole_t *cur_hole = direction > 0 ? ns_list_get_first(&holes_list)
00185                                           : ns_list_get_last(&holes_list);
00186          cur_hole;
00187          cur_hole = direction > 0 ? ns_list_get_next(&holes_list, cur_hole)
00188                                   : ns_list_get_previous(&holes_list, cur_hole)
00189         ) {
00190         int *p = block_start_from_hole(cur_hole);
00191         if (ns_block_validate(p, direction) != 0 || *p >= 0) {
00192             //Validation failed, or this supposed hole has positive (allocated) size
00193             heap_failure(NS_DYN_MEM_HEAP_SECTOR_CORRUPTED);
00194             break;
00195         }
00196         if (-*p >= data_size) {
00197             // Found a big enough block
00198             block_ptr = p;
00199             break;
00200         }
00201     }
00202 
00203     if (!block_ptr) {
00204         goto done;
00205     }
00206 
00207     int block_data_size = -*block_ptr;
00208     if (block_data_size >= (data_size + 2 + HOLE_T_SIZE)) {
00209         int hole_size = block_data_size - data_size - 2;
00210         int *hole_ptr;
00211         //There is enough room for a new hole so create it first
00212         if ( direction > 0 ) {
00213             hole_ptr = block_ptr + 1 + data_size + 1;
00214             // Hole will be left at end of area.
00215             // Would like to just replace this block_ptr with new descriptor, but
00216             // they could overlap, so ns_list_replace might fail
00217             //ns_list_replace(&holes_list, block_ptr, hole_from_block_start(hole_ptr));
00218             hole_t *before = ns_list_get_previous(&holes_list, hole_from_block_start(block_ptr));
00219             ns_list_remove(&holes_list, hole_from_block_start(block_ptr));
00220             if (before) {
00221                 ns_list_add_after(&holes_list, before, hole_from_block_start(hole_ptr));
00222             } else {
00223                 ns_list_add_to_start(&holes_list, hole_from_block_start(hole_ptr));
00224             }
00225         } else {
00226             hole_ptr = block_ptr;
00227             // Hole remains at start of area - keep existing descriptor in place.
00228             block_ptr += 1 + hole_size + 1;
00229         }
00230 
00231         hole_ptr[0] = -hole_size;
00232         hole_ptr[1 + hole_size] = -hole_size;
00233     } else {
00234         // Not enough room for a left-over hole, so use the whole block
00235         data_size = block_data_size;
00236         ns_list_remove(&holes_list, hole_from_block_start(block_ptr));
00237     }
00238     block_ptr[0] = data_size;
00239     block_ptr[1 + data_size] = data_size;
00240 
00241  done:
00242     if (mem_stat_info_ptr) {
00243         if (block_ptr) {
00244             //Update Allocate OK
00245             dev_stat_update(DEV_HEAP_ALLOC_OK, (data_size + 2) * sizeof(int));
00246 
00247         } else {
00248             //Update Allocate Fail, second parameter is not used for stats
00249             dev_stat_update(DEV_HEAP_ALLOC_FAIL, 0);
00250         }
00251     }
00252     platform_exit_critical();
00253 
00254     return block_ptr ? block_ptr + 1 : NULL;
00255 #else
00256     void *retval = NULL;
00257     if (alloc_size) {
00258         platform_enter_critical();
00259         retval = malloc(alloc_size);
00260         platform_exit_critical();
00261     }
00262     return retval;
00263 #endif
00264 }
00265 
00266 void *ns_dyn_mem_alloc(int16_t alloc_size)
00267 {
00268     return ns_dyn_mem_internal_alloc(alloc_size, -1);
00269 }
00270 
00271 void *ns_dyn_mem_temporary_alloc(int16_t alloc_size)
00272 {
00273     return ns_dyn_mem_internal_alloc(alloc_size, 1);
00274 }
00275 
00276 #ifndef STANDARD_MALLOC
00277 static void ns_free_and_merge_with_adjacent_blocks(int *cur_block, int data_size)
00278 {
00279     // Theory of operation: Block is always in form | Len | Data | Len |
00280     // So we need to check length of previous (if current not heap start)
00281     // and next (if current not heap end) blocks. Negative length means
00282     // free memory so we can merge freed block with those.
00283 
00284     hole_t *existing_start = NULL;
00285     hole_t *existing_end = NULL;
00286     int *start = cur_block;
00287     int *end = cur_block + data_size + 1;
00288     //invalidate current block
00289     *cur_block = -data_size;
00290     *end = -data_size;
00291     int merged_data_size = data_size;
00292 
00293     if (cur_block != heap_main) {
00294         cur_block--;
00295         if (*cur_block < 0) {
00296             merged_data_size += (2 - *cur_block);
00297             start -= (2 - *cur_block);
00298             if (-*start >= HOLE_T_SIZE) {
00299                 existing_start = hole_from_block_start(start);
00300             }
00301         }
00302         cur_block++;
00303     }
00304 
00305     if (end != heap_main_end) {
00306         end++;
00307         if (*end < 0) {
00308             merged_data_size += (2 - *end);
00309             if (-*end >= HOLE_T_SIZE) {
00310                 existing_end = hole_from_block_start(end);
00311             }
00312             end += (1 - *end);
00313         }else{
00314             end--;
00315         }
00316     }
00317 
00318     hole_t *to_add = hole_from_block_start(start);
00319     hole_t *before = NULL;
00320     if (existing_end) {
00321         // Extending hole described by "existing_end" downwards.
00322         // Will replace with descriptor at bottom of merged block.
00323         // (Can't use ns_list_replace, because of danger of overlap)
00324         // Optimisation - note our position for insertion below.
00325         before = ns_list_get_next(&holes_list, existing_end);
00326         ns_list_remove(&holes_list, existing_end);
00327     }
00328     if (existing_start) {
00329         // Extending hole described by "existing_start" upwards.
00330         // No need to modify that descriptor - it remains at the bottom
00331         // of the merged block to describe it.
00332     } else {
00333         // Didn't find adjacent descriptors, but may still
00334         // be merging with small blocks without descriptors.
00335         if ( merged_data_size >= HOLE_T_SIZE ) {
00336             // Locate hole position in list, if we don't already know
00337             // from merging with the block above.
00338             if (!existing_end) {
00339                 ns_list_foreach(hole_t, ptr, &holes_list) {
00340                     if (ptr > to_add) {
00341                         before = ptr;
00342                         break;
00343                     }
00344                 }
00345             }
00346             if (before) {
00347                 ns_list_add_before(&holes_list, before, to_add);
00348             } else {
00349                 ns_list_add_to_end(&holes_list, to_add);
00350             }
00351 
00352         }
00353     }
00354     *start = -merged_data_size;
00355     *end = -merged_data_size;
00356 }
00357 #endif
00358 
00359 void ns_dyn_mem_free(void *block)
00360 {
00361 #ifndef STANDARD_MALLOC
00362     int *ptr = block;
00363     int size;
00364 
00365     if (!block) {
00366         return;
00367     }
00368 
00369     if (!heap_main) {
00370         heap_failure(NS_DYN_MEM_HEAP_SECTOR_UNITIALIZED);
00371         return;
00372     }
00373 
00374     platform_enter_critical();
00375     ptr --;
00376     //Read Current Size
00377     size = *ptr;
00378     if (size < 0) {
00379         heap_failure(NS_DYN_MEM_DOUBLE_FREE);
00380     } else if (ptr < heap_main || ptr >= heap_main_end) {
00381         heap_failure(NS_DYN_MEM_POINTER_NOT_VALID);
00382     } else if ((ptr + size) >= heap_main_end) {
00383         heap_failure(NS_DYN_MEM_POINTER_NOT_VALID);
00384     } else {
00385         if (ns_block_validate(ptr, 1) != 0) {
00386             heap_failure(NS_DYN_MEM_HEAP_SECTOR_CORRUPTED);
00387         } else {
00388             ns_free_and_merge_with_adjacent_blocks(ptr, size);
00389             if (mem_stat_info_ptr) {
00390                 //Update Free Counter
00391                 dev_stat_update(DEV_HEAP_FREE, (size + 2) * sizeof(int));
00392             }
00393         }
00394     }
00395     platform_exit_critical();
00396 #else
00397     platform_enter_critical();
00398     free(block);
00399     platform_exit_critical();
00400 #endif
00401 }