uintptr_t* kalloc(uint32_t asize)
{
uint32_t size;
if(!kernel_heap)
return 0;
// Align allocation size on uint32_t
uint8_t mod = asize % 4;
if(mod > 0)
size = asize + 4 - (uint32_t)mod;
else
size = asize;
uint32_t required_size = size + 2*(sizeof(block_footer_t) + sizeof(block_header_t));
block_header_t* blk = kernel_heap->heap_begin;
// Iterate in our heap, and stops when we found a block, or there is no remaining blocks
while((blk->size < required_size ||blk->flags == 1) && next_block(blk))
blk = next_block(blk);
// We found a block !
if(blk->size >= required_size && blk->flags == 0 && blk->magic == HEAP_HEADER_MAGIC)
{
if(blk->size == required_size) {
blk->flags = 1;
return block_data_space(blk);
} else {
// Update block data
block_footer_t* footer = get_footer(blk);
uint32_t old_size = blk->size;
blk->size = size + sizeof(block_header_t) + sizeof(block_footer_t);
blk->flags = 1;
// Create footer for block
block_footer_t* new_footer = (block_footer_t*)((uintptr_t)blk + blk->size - sizeof(block_footer_t));
new_footer->header = blk;
new_footer->magic = HEAP_FOOTER_MAGIC;
// Create new block next to it
block_header_t* new_block = (block_header_t*)((uintptr_t)new_footer + sizeof(block_footer_t));
new_block->magic = HEAP_HEADER_MAGIC;
new_block->flags = 0;
new_block->size = old_size - blk->size;
// Update footer data to link to the newly created block
footer->header = new_block;
block_footer_t* check = get_footer(new_block);
block_footer_t* check2 = get_footer(blk);
return block_data_space(blk);
}
} else {
panic("Kernel heap out of memory\n", 0);
return 0;
}
}
/**
* Free memory
*/
void free(void* pos) {
uint32_t* block = reinterpret_cast<uint32_t*>(pos);
block--;
if (!is_allocated(next_block(block))) {
uint32_t new_size = size_of_block(block) + 4 + size_of_block(next_block(block));
record(block, new_size, kFree);
} else {
record(block, size_of_block(block), kFree);
}
}
// DM_write: 70734 events, 0 overruns, 167806us elapsed, 2us avg, min 1us max 34us 0.620us rms
bool DataFlash_MAVLink::WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical)
{
if (!WritesOK()) {
return false;
}
if (!semaphore->take_nonblocking()) {
dropped++;
return false;
}
if (! WriteBlockCheckStartupMessages()) {
semaphore->give();
return false;
}
if (bufferspace_available() < size) {
if (_startup_messagewriter->finished()) {
// do not count the startup packets as being dropped...
dropped++;
}
semaphore->give();
return false;
}
uint16_t copied = 0;
while (copied < size) {
if (_current_block == nullptr) {
_current_block = next_block();
if (_current_block == nullptr) {
// should not happen - there's a sanity check above
internal_error();
semaphore->give();
return false;
}
}
uint16_t remaining_to_copy = size - copied;
uint16_t _curr_remaining = remaining_space_in_current_block();
uint16_t to_copy = (remaining_to_copy > _curr_remaining) ? _curr_remaining : remaining_to_copy;
memcpy(&(_current_block->buf[_latest_block_len]), &((const uint8_t *)pBuffer)[copied], to_copy);
copied += to_copy;
_latest_block_len += to_copy;
if (_latest_block_len == MAVLINK_MSG_REMOTE_LOG_DATA_BLOCK_FIELD_DATA_LEN) {
//block full, mark it to be sent:
enqueue_block(_blocks_pending, _current_block);
_current_block = next_block();
}
}
semaphore->give();
return true;
}
/* The size of the heap, not including the last block if it's free */
CELL heap_size(F_HEAP *heap)
{
F_BLOCK *scan = first_block(heap);
while(next_block(heap,scan) != NULL)
scan = next_block(heap,scan);
/* this is the last block in the heap, and it is free */
if(scan->status == B_FREE)
return (CELL)scan - heap->segment->start;
/* otherwise the last block is allocated */
else
return heap->segment->size;
}
/* The size of the heap, not including the last block if it's free */
cell heap::heap_size()
{
heap_block *scan = first_block();
while(next_block(scan) != NULL)
scan = next_block(scan);
/* this is the last block in the heap, and it is free */
if(scan->type() == FREE_BLOCK_TYPE)
return (cell)scan - seg->start;
/* otherwise the last block is allocated */
else
return seg->size;
}
/**
* Scan a region of memory and mark any items in the used list appropriately.
* Both arguments should be word aligned.
*/
void mark_from_region(void *start_ptr, void *end_ptr)
{
header_t *block_ptr;
void *current_ptr;
//Iterate word-wise throught the memory
for(current_ptr = start_ptr; current_ptr < end_ptr; current_ptr += sizeof(void*))
{
ptr_int value;
value = *(ptr_int*)current_ptr;
//Iterate thought used memory blocks
block_ptr = usedptr;
do
{
//If pointer value point somewhere into this allocate block
if((ptr_int)start_of_block(block_ptr) <= value
&& (ptr_int)end_of_block(block_ptr) > value)
{
tag(block_ptr);
break;
}
block_ptr = next_block(block_ptr);
}while(block_ptr != NULL);
}
}
/* Compute total sum of sizes of free blocks, and size of largest free block */
void heap_usage(F_HEAP *heap, CELL *used, CELL *total_free, CELL *max_free)
{
*used = 0;
*total_free = 0;
*max_free = 0;
F_BLOCK *scan = first_block(heap);
while(scan)
{
switch(scan->status)
{
case B_ALLOCATED:
*used += scan->size;
break;
case B_FREE:
*total_free += scan->size;
if(scan->size > *max_free)
*max_free = scan->size;
break;
default:
critical_error("Invalid scan->status",(CELL)scan);
}
scan = next_block(heap,scan);
}
}
/* After code GC, all referenced code blocks have status set to B_MARKED, so any
which are allocated and not marked can be reclaimed. */
void free_unmarked(F_HEAP *heap)
{
F_BLOCK *prev = NULL;
F_BLOCK *scan = first_block(heap);
while(scan)
{
switch(scan->status)
{
case B_ALLOCATED:
if(prev && prev->status == B_FREE)
prev->size += scan->size;
else
{
scan->status = B_FREE;
prev = scan;
}
break;
case B_FREE:
if(prev && prev->status == B_FREE)
prev->size += scan->size;
break;
case B_MARKED:
scan->status = B_ALLOCATED;
prev = scan;
break;
default:
critical_error("Invalid scan->status",(CELL)scan);
}
scan = next_block(heap,scan);
}
build_free_list(heap,heap->segment->size);
}
/*** function free_block ***
Deallocate a target block.
Input:
# Heap heap : The target heap
# char** input : The users command-line input
input[0] : The name of this program
input[1] : The number of the block which will be deallocated.
Returns: void
*/
void free_block(char*heap, char** input) {
int blockDelete = atoi(input[1]);
size_t blockID;
size_t size;
bool allocated;
char *point = heap;
size_t totalSize = 0;
if(blockDelete <= 0) {
puts("Invalid block number.");
return;
}
read_block((header_t*) point, &size, &allocated, &blockID);
while(blockID != blockDelete){
point = (char*) next_block((header_t*) point);
read_block((header_t*) point, &size, &allocated, &blockID);
totalSize += size;
if (blockID == 0 || totalSize >= 400){
puts("Invalid block number.");
return;
}
}
if (blockID == blockDelete){
if (!allocated){
puts("Invalid block number.");
return;
}
*(header_t*)point = *(header_t*)point & ~0x8000;
}
}
/*** function print_blocklist ***
Print a list of all blocks in the heap.
Input:
# Heap heap : The target heap
# char** input : The users command-line input
input[0] : The name of this program
Returns: void
*/
void print_blocklist(char*heap, char** input) {
size_t size; // The size of a single block.
size_t blockID;
bool allocated; // The allocation status of a single block.
char* start = heap; // The starting address of a single block.
char* end; // The ending address of a single block
size_t totalSize = 0; // The total amount of blocks we have examined.
// printf("Size\tAllocated\tStart\tEnd\t\n");
printf("%-7s%-12s%-16s%-16s\n", "Size", "Allocated", "Start", "End");
// Loop through the blocks
while(*(header_t*)start) { // && blockID !=0
// Read in the target block.
read_block((header_t*) start, &size, &allocated, &blockID);
// Loop until we are out of the bounds of our heap. (400 blocks).
totalSize += size;
if(totalSize >= 400) {
break;
}
// Point to it's end.
end = start + size - 1;
// Print it's information to stdout.
printf("%-7ld%-12s%-16p%-16p\n", size, allocated == true ? "yes" : "no", start, end);
// Advance to the next block.
start = (char*) next_block((header_t*) start);
}
}
/* Compute total sum of sizes of free blocks, and size of largest free block */
void heap::heap_usage(cell *used, cell *total_free, cell *max_free)
{
*used = 0;
*total_free = 0;
*max_free = 0;
heap_block *scan = first_block();
while(scan)
{
switch(scan->status)
{
case B_ALLOCATED:
*used += scan->size;
break;
case B_FREE:
*total_free += scan->size;
if(scan->size > *max_free)
*max_free = scan->size;
break;
default:
myvm->critical_error("Invalid scan->status",(cell)scan);
}
scan = next_block(scan);
}
}
/* Write a block of data at current offset */
bool DataFlash_MAVLink::WriteBlock(const void *pBuffer, uint16_t size)
{
if (!_initialised || !_sending_to_client || !_writes_enabled) {
return false;
}
if (! WriteBlockCheckPrefaceMessages()) {
return false;
}
if (bufferspace_available() < size) {
if (_startup_messagewriter->finished()) {
// do not count the startup packets as being dropped...
dropped++;
}
return false;
}
uint16_t copied = 0;
while (copied < size) {
if (_current_block == NULL) {
_current_block = next_block();
if (_current_block == NULL) {
// should not happen - there's a sanity check above
internal_error();
return false;
}
}
uint16_t remaining_to_copy = size - copied;
uint16_t _curr_remaining = remaining_space_in_current_block();
uint16_t to_copy = (remaining_to_copy > _curr_remaining) ? _curr_remaining : remaining_to_copy;
memcpy(&(_current_block->buf[_latest_block_len]), &((const uint8_t *)pBuffer)[copied], to_copy);
copied += to_copy;
_latest_block_len += to_copy;
if (_latest_block_len == MAVLINK_MSG_REMOTE_LOG_DATA_BLOCK_FIELD_DATA_LEN) {
//block full, mark it to be sent:
enqueue_block(_blocks_pending, _current_block);
_current_block = next_block();
}
}
if (!_writing_preface_messages) {
// push_log_blocks();
}
return true;
}
/**
* Scans the heap for active pointers and marks ones that points somewhere
*/
void mark_from_heap(void)
{
header_t *current_ptr;
for(current_ptr = usedptr; current_ptr != NULL; current_ptr = next_block(current_ptr))
{
mark_from_region(start_of_block(current_ptr), end_of_block(current_ptr));
}
}
void heap::clear_mark_bits()
{
heap_block *scan = first_block();
while(scan)
{
scan->set_marked_p(false);
scan = next_block(scan);
}
}
/*
* free - free a allocated block
*/
void free(void *bp) {
if (bp == NULL) {
return;
}
dbg_printf("want to free %d size block in address 0x%lx\n", (int)block_size(bp), (long)bp);
print_heap();
if (block_alloc(bp) == 0) {
return;
}
if (heap_head == NULL) {
mm_init();
}
mark(bp, block_size(bp), block_prev_alloc(bp), 0);
mark(next_block(bp), block_size(next_block(bp)), 0, block_alloc(next_block(bp)));
insert_to_list(bp);
bp = coalesce(bp);
dbg_printf("want return from free %d size block in address 0x%lx\n", (int)block_size(bp), (long)bp);
print_heap();
}
/* Apply a function to every code block */
void iterate_code_heap(CODE_HEAP_ITERATOR iter)
{
F_BLOCK *scan = first_block(&code_heap);
while(scan)
{
if(scan->status != B_FREE)
iterate_code_heap_step(block_to_compiled(scan),iter);
scan = next_block(&code_heap,scan);
}
}
/*
* place - transform a free block to an allocated block,
* then delete old free block from its free list
* and insert the rest part to the corresponding free list
*/
static void place(void *bp, size_t alloc_block_size) {
size_t size;
delete_from_list(bp);
size = block_size(bp);
dbg_printf("want to place a %d size allocated block from a %d size free block\n", (int)alloc_block_size, (int)size);
if (size - alloc_block_size >= 4 * WSIZE) {
/* have rest part for a new free block */
if (last_block == bp) {
last_block = bp + alloc_block_size;
}
mark(bp, alloc_block_size, block_prev_alloc(bp), 1);
bp += alloc_block_size;
mark(bp, size - alloc_block_size, 1, 0);
insert_to_list(bp);
} else {
/* have no rest part for a new free block */
mark(bp, size, block_prev_alloc(bp), 1);
mark(next_block(bp), block_size(next_block(bp)), 1, block_alloc(next_block(bp)));
}
}
/* If in the middle of code GC, we have to grow the heap, data GC restarts from
scratch, so we have to unmark any marked blocks. */
void heap::unmark_marked()
{
heap_block *scan = first_block();
while(scan)
{
if(scan->status == B_MARKED)
scan->status = B_ALLOCATED;
scan = next_block(scan);
}
}
/* If in the middle of code GC, we have to grow the heap, GC restarts from
scratch, so we have to unmark any marked blocks. */
void unmark_marked(F_HEAP *heap)
{
F_BLOCK *scan = first_block(heap);
while(scan)
{
if(scan->status == B_MARKED)
scan->status = B_ALLOCATED;
scan = next_block(heap,scan);
}
}
/* Called after reading the code heap from the image file, and after code GC.
In the former case, we must add a large free block from compiling.base + size to
compiling.limit. */
void build_free_list(F_HEAP *heap, CELL size)
{
F_BLOCK *prev = NULL;
F_BLOCK *prev_free = NULL;
F_BLOCK *scan = first_block(heap);
F_BLOCK *end = (F_BLOCK *)(heap->segment->start + size);
/* Add all free blocks to the free list */
while(scan && scan < end)
{
switch(scan->status)
{
case B_FREE:
update_free_list(heap,prev_free,scan);
prev_free = scan;
break;
case B_ALLOCATED:
break;
default:
critical_error("Invalid scan->status",(CELL)scan);
break;
}
prev = scan;
scan = next_block(heap,scan);
}
/* If there is room at the end of the heap, add a free block. This
branch is only taken after loading a new image, not after code GC */
if((CELL)(end + 1) <= heap->segment->end)
{
end->status = B_FREE;
end->next_free = NULL;
end->size = heap->segment->end - (CELL)end;
/* add final free block */
update_free_list(heap,prev_free,end);
}
/* This branch is taken if the newly loaded image fits exactly, or
after code GC */
else
{
/* even if there's no room at the end of the heap for a new
free block, we might have to jigger it up by a few bytes in
case prev + prev->size */
if(prev)
prev->size = heap->segment->end - (CELL)prev;
/* this is the last free block */
update_free_list(heap,prev_free,NULL);
}
}
static void print_heap() {
int i, block_count;
unsigned *list_p;
void *bp;
size_t size, alloc;
if (DEBUG_CHECKHEAP) {
printf("\n----- arrays -----\n");
list_p = (unsigned *)heap_head;
for (i = 0; i <= 4; i++) {
printf("0x%lx: [%d, %d] -> %x\n", (long)list_p, i + 4, i + 4, (unsigned)*list_p);
list_p++;
}
for (; i < ARRAYSIZE; i++) {
printf("0x%lx: [%d, %d] -> %x\n", (long)list_p, (1 << (i - 2)) + 1, 1 << (i - 1), (unsigned)*list_p);
list_p++;
}
printf("----- blocks -----\n");
bp = data_head + 4 * WSIZE;
size = block_size(bp);
alloc = block_alloc(bp);
block_count = 0;
while (!size == 0) {
printf("\n----- block %d -----\n", block_count);
block_count++;
printf("bp: 0x%lx\n", (long)bp);
printf("--- header ---\n");
printf("size: %d\n", (int)size);
printf("prev alloc: %d\n", (int)block_prev_alloc(bp));
printf("alloc: %d\n", (int)alloc);
printf("--- data ---\n");
if (alloc == 0) { // free block
printf("- pointer -\n");
printf("pred: 0x%lx -> 0x%x\n", (long)bp, *(unsigned *)bp);
printf("succ: 0x%lx -> 0x%x\n", (long)(bp + WSIZE), *((unsigned *)bp + 1));
} else { // allocated block
/*for (i = 0; i < size - DSIZE; i += WSIZE) {
printf("data[%d] = %d\n", i / WSIZE, *((int *)(bp + i)));
}*/
printf("data[%d~%d]\n", 0, (int)size / WSIZE - 2);
}
/*
printf("--- footer ---\n");
printf("size: %d\n", (int)size);
printf("prev alloc: %d\n", (int)block_prev_alloc(bp));
printf("alloc: %d\n", (int)alloc);
*/
bp = next_block(bp);
size = block_size(bp);
alloc = block_alloc(bp);
}
printf("last_block: %lx\n\n\n", (long)last_block);
}
}
void heap::compact_heap()
{
heap_block *scan = first_block();
while(scan)
{
heap_block *next = next_block(scan);
if(scan->type() != FREE_BLOCK_TYPE)
memmove(forwarding[scan],scan,scan->size());
scan = next;
}
}
void heap::compact_heap(unordered_map<heap_block *,char *> &forwarding)
{
heap_block *scan = first_block();
while(scan)
{
heap_block *next = next_block(scan);
if(scan->status == B_ALLOCATED)
memmove(forwarding[scan],scan,scan->size);
scan = next;
}
}
void compact_heap(F_HEAP *heap)
{
F_BLOCK *scan = first_block(heap);
while(scan)
{
F_BLOCK *next = next_block(heap,scan);
if(scan->status == B_ALLOCATED && scan != scan->forwarding)
memcpy(scan->forwarding,scan,scan->size);
scan = next;
}
}
/**
* Retrieves from the list first block of fitting size
* @arg list_ptr pointer to the start pointer of a list
* @arg size minimum searched size
*/
header_t *first_fit(header_t **list_ptr, size_t size)
{
header_t *previous_ptr = NULL, *current_ptr;
for(current_ptr = *list_ptr; current_ptr != NULL; current_ptr = next_block(current_ptr))
{
if(available_size(current_ptr) >= size)
break;
previous_ptr = current_ptr;
}
return remove_from_list(list_ptr, previous_ptr, current_ptr);
}
/* Called after reading the code heap from the image file, and after code GC.
In the former case, we must add a large free block from compiling.base + size to
compiling.limit. */
void heap::build_free_list(cell size)
{
heap_block *prev = NULL;
clear_free_list();
size = (size + block_size_increment - 1) & ~(block_size_increment - 1);
heap_block *scan = first_block();
free_heap_block *end = (free_heap_block *)(seg->start + size);
/* Add all free blocks to the free list */
while(scan && scan < (heap_block *)end)
{
switch(scan->status)
{
case B_FREE:
add_to_free_list((free_heap_block *)scan);
break;
case B_ALLOCATED:
break;
default:
myvm->critical_error("Invalid scan->status",(cell)scan);
break;
}
prev = scan;
scan = next_block(scan);
}
/* If there is room at the end of the heap, add a free block. This
branch is only taken after loading a new image, not after code GC */
if((cell)(end + 1) <= seg->end)
{
end->status = B_FREE;
end->size = seg->end - (cell)end;
/* add final free block */
add_to_free_list(end);
}
/* This branch is taken if the newly loaded image fits exactly, or
after code GC */
else
{
/* even if there's no room at the end of the heap for a new
free block, we might have to jigger it up by a few bytes in
case prev + prev->size */
if(prev) prev->size = seg->end - (cell)prev;
}
}
/**
* Appends new block to end of the list
* @arg list_ptr pointer to the start pointer of a list
* @arg new_ptr pointer to the new block of memory added
*/
void add_to_list(header_t **list_ptr, header_t *new_ptr)
{
header_t *current_ptr, *previous_ptr = NULL;
for(current_ptr = *list_ptr; current_ptr != NULL; current_ptr = next_block(current_ptr))
{
previous_ptr = current_ptr;
}
if(previous_ptr == NULL)
*list_ptr = new_ptr;
else
previous_ptr->next = new_ptr;
new_ptr->next = NULL;
}
/* Compute where each block is going to go, after compaction */
cell heap::compute_heap_forwarding()
{
heap_block *scan = first_block();
char *address = (char *)first_block();
while(scan)
{
if(scan->type() != FREE_BLOCK_TYPE)
{
forwarding[scan] = address;
address += scan->size();
}
scan = next_block(scan);
}
return (cell)address - seg->start;
}
/*
* extend_heap - extend heap with a new free block and return its block pointer
*/
static void *extend_heap(size_t size) {
void *bp;
dbg_printf("want to extend heap by %d size\n", (int)size);
bp = mem_sbrk(size);
if (bp == (void *)-1) {
return NULL;
}
if (last_block == NULL) {
last_block = bp;
mark(bp, size, 1, 0);
} else {
mark(bp, size, block_alloc(last_block), 0);
last_block = bp;
}
PUT(HEAD(next_block(bp)), PACK(0, 0, 1)); /* set new epilogue */
insert_to_list(bp);
bp = coalesce(bp);
return bp;
}
/* Called after reading the code heap from the image file, and after code GC.
In the former case, we must add a large free block from compiling.base + size to
compiling.limit. */
void heap::build_free_list(cell size)
{
heap_block *prev = NULL;
clear_free_list();
size = (size + block_size_increment - 1) & ~(block_size_increment - 1);
heap_block *scan = first_block();
free_heap_block *end = (free_heap_block *)(seg->start + size);
/* Add all free blocks to the free list */
while(scan && scan < (heap_block *)end)
{
if(scan->type() == FREE_BLOCK_TYPE)
add_to_free_list((free_heap_block *)scan);
prev = scan;
scan = next_block(scan);
}
/* If there is room at the end of the heap, add a free block. This
branch is only taken after loading a new image, not after code GC */
if((cell)(end + 1) <= seg->end)
{
end->set_marked_p(false);
end->set_type(FREE_BLOCK_TYPE);
end->set_size(seg->end - (cell)end);
/* add final free block */
add_to_free_list(end);
}
/* This branch is taken if the newly loaded image fits exactly, or
after code GC */
else
{
/* even if there's no room at the end of the heap for a new
free block, we might have to jigger it up by a few bytes in
case prev + prev->size */
if(prev) prev->set_size(seg->end - (cell)prev);
}
}