mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
AP_HAL_ChibiOS: Use a less clever version of realloc
The clever version loses access to memory, and is excessively hard coded
This commit is contained in:
parent
effdc436f2
commit
8c464572d8
@ -216,45 +216,21 @@ size_t mem_available(void)
|
||||
return totalp;
|
||||
}
|
||||
|
||||
/*
|
||||
realloc implementation thanks to wolfssl, used by AP_Scripting
|
||||
*/
|
||||
void *realloc(void *addr, size_t size)
|
||||
{
|
||||
union heap_header *hp;
|
||||
uint32_t prev_size, new_size;
|
||||
|
||||
void *ptr;
|
||||
|
||||
if(addr == NULL) {
|
||||
return chHeapAlloc(NULL, size);
|
||||
if (size == 0) {
|
||||
free(addr);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/* previous allocated segment is preceded by an heap_header */
|
||||
hp = addr - sizeof(union heap_header);
|
||||
prev_size = hp->used.size; /* size is always multiple of 8 */
|
||||
|
||||
/* check new size memory alignment */
|
||||
if (size % 8 == 0) {
|
||||
new_size = size;
|
||||
} else {
|
||||
new_size = ((int) (size / 8)) * 8 + 8;
|
||||
if (addr == NULL) {
|
||||
return malloc(size);
|
||||
}
|
||||
|
||||
if(prev_size >= new_size) {
|
||||
return addr;
|
||||
void *new_mem = malloc(size);
|
||||
if (new_mem != NULL) {
|
||||
memcpy(new_mem, addr, chHeapGetSize(addr) > size ? size : chHeapGetSize(addr));
|
||||
free(addr);
|
||||
}
|
||||
|
||||
ptr = chHeapAlloc(NULL, size);
|
||||
if (ptr == NULL) {
|
||||
return NULL;
|
||||
}
|
||||
|
||||
memcpy(ptr, addr, prev_size);
|
||||
|
||||
chHeapFree(addr);
|
||||
|
||||
return ptr;
|
||||
return new_mem;
|
||||
}
|
||||
|
||||
#endif // CH_CFG_USE_HEAP
|
||||
|
Loading…
Reference in New Issue
Block a user