prototype allocate function

include/util/arena_alloc.nasm

@@ -14,10 +14,10 @@ endstruc
 arena_init:
     __CDECL16_ENTRY
 .func:
-    movzx eax, word [bp + 4]     ; ptr to state structure
+    mov ax, word [bp + 4]     ; ptr to state structure
     mov di, ax
 
-    xor eax, eax
+    xor ax, ax
     mov word [di + ArenaStateStruc_t.mark], ax
     mov word [di + ArenaStateStruc_t.end], word (__ARENA_HEAP_START + __ARENA_HEAP_SIZE)
     mov word [di + ArenaStateStruc_t.start], __ARENA_HEAP_START
@@ -46,14 +46,23 @@ arena_init:
 arena_align_up:
     __CDECL16_ENTRY
 .func:
-    
+    ; if a == 0 return x
     mov ax, [bp + 4]      ; x
+    mov bx, [bp + 6]      ; a
 
-    mov bx, [bp + 6]      
-    sub bx, 1             ; a - 1
+    or bx, bx
+    jz .endp              
+
+    ; enforce power-of-two for alignment, return x
+    mov cx, bx
+    dec cx
+    test bx, cx
+    jnz .endp
+
+    dec bx                ; a - 1
 
     mov cx, ax
-    add cx, bx            ; x + (a+1)
+    add cx, bx            ; x + (a-1)
     not bx                ; ~(a-1)
 
     and cx, bx            ; and with the inverse
@@ -63,16 +72,59 @@ arena_align_up:
     __CDECL16_EXIT
     ret
 
-; void* arena_alloc(void* a, size_t bytes, size_t align)
+; void* arena_alloc(size_t bytes, size_t align)
+; bp-2 - current used arena (i.e 'highmark')
+; bp-4 - aligned_ptr 
+; bp-6 - new_end
 arena_alloc:
-    __CDECL16_ENTRY
+    __CDECL16_ENTRY 0x10
 .func:
     ; remove bytes from pool and increment mark to the new cursor location
     ; return a pointer to the begining of  allocated segment
-    ERROR STEVIA_DEBUG_UNIMPLEMENTED
+    mov bx, early_heap_state
+    
+    mov ax, word [bx + ArenaStateStruc_t.start]
+    mov dx, word [bx + ArenaStateStruc_t.mark]
+    add ax, dx                      ; i.e the total used arena
+    mov word [bp - 2], ax           ; save as a local
+
+    push bx                         ; save heap_state pointer
+
+    mov ax, word [bp + 6]           ; requested next allocation alignment
+    push ax
+    mov ax, word [bp - 2]           ; current arena 'highmark'
+    push ax
+    call arena_align_up
+    add sp, 0x4
+    mov word [bp - 4], ax           ; save return value
+
+    pop bx                          ; restore heap_state pointer
+
+    ; new_end = aligned_ptr + bytes 
+    add ax, word [bp + 4]           ; add to total (so current aligned mark + bytes)
+    mov word [bp - 6], ax           ; save as local
+
+    mov dx, word [bx + ArenaStateStruc_t.end]
+    cmp ax, dx
+    ja arena_alloc.error            ; if our heap end is < the requested throw an error, heap is full
+                                    ; else update the mark to the new value & return  the aligned pointer
+
+    ; mark_delta = new_end - curr_used
+    mov dx, word [bp - 6]
+    sub dx, word [bp - 2]
+
+    ; mark += mark_delta
+    mov ax, word [bx + ArenaStateStruc_t.mark]
+    add ax, dx
+    mov word [bx + ArenaStateStruc_t.mark], ax
+    
+    ; return aligned_ptr
+    mov ax, word [bp - 4]
 .endp:
     __CDECL16_EXIT
     ret
+.error:
+    ERROR STEVIA_DEBUG_ERR
 
 arena_mark:
     __CDECL16_ENTRY