AP_UAVCAN: added a check for memory corruption in the pool

This commit is contained in:
Andrew Tridgell 2022-06-03 14:32:58 +10:00
parent 541de85354
commit 540dcc554f

View File

@ -51,6 +51,11 @@ void* AP_PoolAllocator::allocate(std::size_t size)
return nullptr; return nullptr;
} }
Node *ret = free_list; Node *ret = free_list;
const uint32_t blk = ret - pool_nodes;
if (blk >= num_blocks) {
INTERNAL_ERROR(AP_InternalError::error_t::mem_guard);
return nullptr;
}
free_list = free_list->next; free_list = free_list->next;
used++; used++;
@ -69,6 +74,11 @@ void AP_PoolAllocator::deallocate(const void* ptr)
WITH_SEMAPHORE(sem); WITH_SEMAPHORE(sem);
Node *p = reinterpret_cast<Node*>(const_cast<void*>(ptr)); Node *p = reinterpret_cast<Node*>(const_cast<void*>(ptr));
const uint32_t blk = p - pool_nodes;
if (blk >= num_blocks) {
INTERNAL_ERROR(AP_InternalError::error_t::mem_guard);
return;
}
p->next = free_list; p->next = free_list;
free_list = p; free_list = p;