include include/syscalls.i
  section .text,text
; Frees the memory block pointed to by a0
; free_w_size internal entry expecting size in d0 
; instead of 4 bytes before passed in address
  public free
free:
  move.l -(a0), d0 ; Read the allocatuon size and get the true hole pointer
  move.l (free_list), a1 ; Get the head of the free list in a0
  cmpa.l #0, a1 ; Check if the free list is empty
  bne.b free_list_not_empty ; If not, find the right position and insert the hole
  move.l d0, (a0) ; Otherwise, set the free list to be the passed in hole and return
  move.l #0, ($4,a0)
  move.l a0, (free_list)
  rts
free_list_not_empty:
  move.l a2, -(a7) ; Preserve a2 (callee saved)
  move.l #0, a1 ; a1 is the pointer to the previous hole in the list, starts out NULL
  move.l (free_list), a2 ; a2 is the pointer to the current hole in the list
free_find_loop:
  cmpa.l #0, a2 ; If we have hit the end of the list, we have found the right position
  beq.b free_find_loop_done
  cmp.l a0, a2
  bgt.b free_find_loop_done
  move.l a2, a1 ; Go to the next item
  move.l ($4,a2), a2
  bra.b free_find_loop
free_find_loop_done:
; We have now found where to put the hole, that being between a1 (previous hole), and a2 (current hole)
  cmpa.l #0, a1 ; If there is no previous item, we cannot merge
  beq.b free_no_prev
  move.l (a1), d1 ; If size of prev hole + address of prev hole = address of hole to free, they are consecutive and can be merged
  add.l a1, d1
  cmp.l a0, d1
  bne.b free_no_prev_merge
  add.l d0, (a1) ; size of prev hole += size of hole to free
  move.l a1, a0 ; hole to free = prev hole
  move.l #0, a1 ; prev hole = NULL
  bra.b free_prev_merge_done
free_no_prev_merge:
  move.l a2, ($4,a0) ; If we are not merging, hole to free next = current hole, prev hole next = hole to free
  move.l a0, ($4,a1)
  bra free_prev_merge_done
free_no_prev:
  move.l (free_list), ($4,a0) ; If there is no previous hole, add it to the beginning of the free list
  move a0, (free_list)
free_prev_merge_done:
  cmpa.l #0, a2 ; If there is no current hole, no merging must be done
  beq.b free_curr_merge_done
  move.l (a0), d1 ; If size of hole to free + address of hole to free = address of current hole, they are consecutive and can be merged 
  add.l a0, d1
  cmp.l a2, d1
  bne.b free_curr_merge_done
  move.l ($4,a2), d1 ; hole to free next = current hole next
  move.l d1, ($4,a0)
  move.l (a0), d1 ; hole to free size += current hole size
  add.l (a2), d1
  move.l d1, (a0)
free_curr_merge_done:
  move.l (a7)+, a2 ; Restore a2 (callee saved)
  rts

; Allocates a memory block of size d0 and returns the address in a0
  public malloc
malloc:
  ; Return NULL if d0=0
  cmpi.l #0, d0
  bne .1
  move.l #0, a0
  rts
.1:
  ; d0=d0+4 rounded to next multiple of 4
  addi.l #4, d0
  move.l d0, d1
  andi.l #$2, d1
  beq.b .2
  move.l d0, d1
  andi.l #~$2, d1
  addi.l #8, d1
  move.l d1, d0
.2:
  move.l (free_list), a0
malloc_find_loop:
  cmpa.l #0, a0
  beq malloc_found_end
  move.l (a0), d1
  cmp.l d0, d1
  blt.b malloc_too_small
  beq.b malloc_block_exact_size
  sub.l d0, d1
  move.l d1, (a0)
  adda.l d1, a0
  move.l d0, (a0)+
  rts
malloc_block_exact_size:
  adda.l #4, a0
  rts
malloc_too_small:
  move.l ($4,a0), a0
  bra.b malloc_find_loop
malloc_found_end:
  move.l d0, -(a7)
  move.l d0, d1
  andi.l #$fff, d1
  beq.b .1
  move.l d0, d1
  andi.l #~$fff, d1
  addi.l #$1000, d1
  move.l d1, d0
.1:
  lsr.l #8, d0
  lsr.l #4, d0
  move.l d0, -(a7)
  jsr alloc_pages
  move.l (a7)+, d0
  lsl.l #8, d0
  lsl.l #4, d0
  move.l d0, d1
  move.l (a7)+, d0
  sub.l d0, d1
  move.l a0, a1
  adda.l d1, a1
  move.l d0, (a1)+
  move.l d1, d0
  move.l a1, -(a7)
  move.l d0, (a0)+
  bsr.w free
  move.l (a7)+, a0
  rts

; Allocate a block of memory d0 pages in length and returns the address in a0
  weak alloc_pages
alloc_pages:
  move.l d2, -(a7)
  move.l #$2, d1
  jsr syscall_vmem_map_free
alloc_pages_done:
  move.l (a7)+, d2 
  rts



  section .bss,bss
free_list:
  ds.b 4