Implement growing of kernel heap

diff --git a/kernel/arch/heap.c b/kernel/arch/heap.c
index c6819d0295bc875bbbf5f8bc4106326be15e458b..6ce6d1783797f38243e54568ee6d2503e6db111f 100644 (file)
--- a/kernel/arch/heap.c
+++ b/kernel/arch/heap.c
@@ -35,6 +35,31 @@ static struct {
        u32_t h_free;
 } _kheap;
 
+#if FEATURE(DEBUG)
+void heap_debug(void)
+{
+       struct heap_node *cur;
+
+       dbg_printf("heap_debug():\n"
+                          " _kheap @ %p\n"
+                          "  h_base = %p\n"
+                          "  h_size = 0x%08x\n"
+                          "  h_free = 0x%08x\n",
+                          &_kheap, _kheap.h_base, _kheap.h_size, _kheap.h_free);
+
+       for(cur = (struct heap_node*)_kheap.h_base;
+               cur;
+               cur = cur->hn_next) {
+               dbg_printf("  node @ %p\n"
+                                  "   hn_size  = 0x%08x\n"
+                                  "   hn_flags = 0x%08x\n",
+                                  cur, cur->hn_size, cur->hn_flags);
+       }
+
+       return;
+}
+#endif /* FEATURE(DEBUG) */
+
 void heap_init(void *base, u32_t size)
 {
        struct heap_node *node;
@@ -52,12 +77,50 @@ void heap_init(void *base, u32_t size)
        return;
 }
 
-int heap_grow(u32_t sreq)
+static int heap_grow(const u32_t size)
 {
+       struct heap_node *cur;
+       struct heap_node *new;
+       u32_t sreq;
        int ret_val;
 
-       ret_val = -1;
-/*     sreq = ALIGN(sreq, PAGE_SIZE); */
+       new = NULL;
+       ret_val = 1;
+
+       /* add more pages to the pagedir */
+
+       for(sreq = ALIGN(size << 2, PAGE_SIZE); sreq > 0; sreq -= PAGE_SIZE) {
+               void *addr;
+
+               addr = pg_frame_alloc_start();
+
+               if(!addr) {
+                       break;
+               }
+
+               if(!new) {
+                       new = (struct heap_node*)addr;
+                       cur = (struct heap_node*)_kheap.h_base;
+
+                       new->hn_size = PAGE_SIZE - sizeof(*new);
+                       new->hn_flags = 0;
+                       new->hn_next = NULL;
+
+                       /* add new node to the end of the heap */
+                       while(cur->hn_next) {
+                               cur = cur->hn_next;
+                       }
+
+                       cur->hn_next = new;
+               } else {
+                       new->hn_size += PAGE_SIZE;
+               }
+       }
+
+       if(new) {
+               _kheap.h_size += new->hn_size;
+               ret_val = 0;
+       }
 
        return(ret_val);
 }
@@ -70,7 +133,7 @@ void* kmalloc(u32_t size)
        ret_val = NULL;
 
        if(_kheap.h_free < size) {
-               /* add more pages to the pagedir */
+               heap_grow(size);
        }
 
        for(cur = (struct heap_node*)_kheap.h_base;
@@ -110,6 +173,10 @@ void* kmalloc(u32_t size)
                break;
        }
 
+#if FEATURE(DEBUG)
+       heap_debug();
+#endif /* FEATURE(DEBUG) */
+
        return(ret_val);
 }
 
@@ -124,7 +191,7 @@ void kfree(void *ptr)
 
                base = (void*)(cur + 1);
 
-               if(base < ptr && (base + cur->hn_size) > ptr) {
+               if(base <= ptr && (base + cur->hn_size) > ptr) {
                        if(!(cur->hn_flags & FLAG_USED)) {
                                dbg_printf("Double free detected: %p\n", ptr);
                        } else {
@@ -135,5 +202,9 @@ void kfree(void *ptr)
                }
        }
 
+#if FEATURE(DEBUG)
+       heap_debug();
+#endif /* FEATURE(DEBUG) */
+
        return;
 }