.global tasking_init
tasking_init:
        ld bc, (next_pid)
        push bc
        pop de
        inc de
        ld (next_pid), de
        push bc
        call allocate_process
        push bc
        pop ix
        pop bc
        push bc
        ld (ix+2), 0x3
        ld (ix+3), 0xF
        ld (ix+4), 0x0
        ld (ix+9), c
        ld (ix+10), b
        push ix
        pop iy
        ld bc, 11
        add iy, bc
        ld c, 0
        task_init_pmap_set_loop:
        call get_frame
        ld (iy+0), b
        ld (iy+1), d
        ld (iy+2), e
        inc iy
        inc iy
        inc iy
        inc c
        cp 12
        jp nz, task_init_pmap_set_loop        
        pop bc
        jp schedule_process

        .global new_process
        ; Creates a new process
        ; Entry point in HL, page mapping pointer in IY
new_process:
        ld bc, (next_pid)
        push bc
        pop de
        inc de
        ld (next_pid), de
        push bc
        push hl
        call allocate_process
        pop hl
        push bc
        pop ix
        pop bc
        push bc
        ld (ix+9), c
        ld (ix+10), b
        ld bc, 0
        ld (ix+5), c
        ld (ix+6), b
        /* ld (ix+7), e */
        /* ld (ix+8), d */
        pop bc
        push hl
        push bc
        push iy
        pop hl
        push ix
        pop iy
        ld de, 11
        add iy, de
        ld b, 12
        new_p_pmap_set_loop:
        ld a, (hl)
        ld (iy+0), a
        inc hl
        ld a, (hl)
        ld (iy+1), a
        inc hl
        ld a, (hl)
        ld (iy+2), a
        inc hl
        inc iy
        inc iy
        inc iy
        djnz new_p_pmap_set_loop
        new_p_sp_set:
        push ix
        call get_free_frame
new_p_after_gff:
        pop ix
        pop de
        push bc
        push de
        pop bc
        push hl
        push de
        call get_process_ptr
new_p_after_gpp:
        pop de
        push bc
        pop ix
        pop hl
        pop bc
        ld (ix+2), c
        ld (ix+3), l
        ld (ix+4), h
        ld (ix+0), 0xEA
        ld (ix+1), 0xFF
        ld b, c
        ld c, 0xE
        push de
        push hl
        pop de
        push ix
        call set_frame
        pop ix
        new_p_ep_set:
        pop bc
        pop hl
        ld (0xEFFE), hl
        ; Schedules the process with the pid in BC
schedule_process:
        ld hl, (ready_to_run_head)
        ld a, h
        cp 0
        jp nz, block1
        ld a, l
        cp 0
        jp nz, block1
        jp end1
block1:
        push bc
        push hl
        pop bc
        call get_process_ptr
        push bc
        pop iy
        pop bc
        push bc
        /* ld (iy+7), c */
        /* ld (iy+8), b */
        call get_process_ptr
        push bc
        pop ix
        pop bc
        ld (ix+5), l
        ld (ix+6), h
        ld (ready_to_run_head), bc
        ret
end1:
        ld hl, (current_process)
        ld a, h
        cp 0
        jp nz, block2
        ld a, l
        cp 0
        jp nz, block2
        jp block3
block2:
        ld (ready_to_run_head), bc
        ret
block3:
        ld (current_process), bc
        ret

.global yield
yield:
        push ix
        ld ix, (ready_to_run_head)
        ld a, ixh
        cp 0
        jp nz, end3
        ld a, ixl
        cp 0
        jp nz, end3
        pop ix
        ret
end3:
        pop ix
        ; Save old process context
        push af
        push bc
        push de
        push hl
        exx
        ex af, af'
        push af
        push bc
        push de
        push hl
        exx
        ex af, af'
        push ix
        push iy
        ld bc, (current_process)
        call get_process_ptr
        push bc
        pop ix
        ld de, 11
        add ix, de
        call pmap_set
        ld bc, (current_process)
        call get_process_ptr
        push bc
        pop iy
        ld hl, 0
        add hl, sp
        ld (iy+0), l
        ld (iy+1), h
yield_list_manip:
        ; Get next process into ix, unlink it from the
        ; ready to run list, and link the current process in
        ; Logic:
        ; next_process = ready_to_run_head -> pid
        ; if (current_process->pid == 1) {
        ; ready_to_run_head = 0
        ; } else {
        ; current_process->next = ready_to_run_head->next
        ; ready_to_run_head = current_process
        ; }
        ; current_process = next_process 
        ld ix, (ready_to_run_head)
        push ix
        pop bc
        call get_process_ptr
ylm_gpp_done:
        push bc
        pop ix
        ld e, (ix+9)
        ld d, (ix+10)
        ld l, (ix+5)
        ld h, (ix+6)
        push hl
        push de
        push ix
        ld bc, (current_process)
        call get_process_ptr
ylm_gpp2_done:
        pop ix
        pop de
        pop hl
        push bc
        pop iy
        ld a, (iy + 9)
        cp 1
        jp nz, else4
        xor a
        ld (ready_to_run_head), a
        ld (ready_to_run_head+1), a
        jp end4
else4:
        ld (iy+5), l
        ld (iy+6), h
        ld bc, (current_process)
        ld (ready_to_run_head), bc
end4:
        ld (current_process), de
yield_sp_page_set:
        push ix
        ld de, 11
        add ix, de
        call pmap_load
ysps_pl_done:
        pop ix
        ; Load next process context
        ld b, (ix+2)
        ld c, 0xF
        ld e, (ix+3)
        ld d, (ix+4)
        ld iy, vmem_mapping_card
        ld (iy+0xF), b
        ld iy, vmem_mapping_bank_low
        ld (iy+0xF), e
        ld iy, vmem_mapping_bank_high
        ld (iy+0xF), d
        ld a, e
        out (c), a
        ld a, c
        or 0x20
        ld c, a
        ld a, d
        or 0x80
        out (c), a
        ld bc, (current_process)
        call get_process_ptr
        push bc
        pop ix
yield_sp_load:
        ld l, (ix+0)
        ld h, (ix+1)
        ld sp, hl
        pop iy
        pop ix
        pop hl
        pop de
        pop bc
        pop af
        exx
        ex af, af'
        pop hl
        pop de
        pop bc
        pop af
        ret

ready_to_run_head: .ds.b 2
current_process: .ds.b 2
next_pid: .dc.w 1
process_allocation_bitmap: .ds.b 128
process_heap_pages: .ds.b 48

; Given a pid BC, return the pointer to the process or 0 if it's nonexistent, mapping the right page in if necessary
; Clobbers A, BC, DE, HL, and IX 
get_process_ptr:
push bc
ld a, c
and 0x7
sla a
sla a
sla a
or 0xC7
ld (set_ins+1), a
; Shift bc right 3 times to get the byte number in c
srl b
rr c
srl b
rr c
srl b
rr c
ld hl, process_allocation_bitmap
add hl, bc
ld a, (hl)
set_ins: set 0, a
ld (hl), a
pop bc
push bc
; Shift bc right 6 times to get the page number in c
push bc
pop hl
xor a
add hl, hl
rla
add hl, hl
rla
ld l, h
ld h, a
push hl
pop bc
; Multiply the page number by 3 to get the start offset in the table
add hl, bc
add hl, bc
push hl
pop bc
ld hl, process_heap_pages
add hl, bc
ld a, (hl)
cp 0
jp z, not_allocated
ld c, 0xE
ld b, (hl)
inc hl
ld e, (hl)
inc hl
ld d, (hl)
call set_frame
pop bc
ld a, c
and 0x3F
ld c, a
ld b, 0
; Shift BC right 6 bits (BC=BC*64)
push bc
pop hl
xor a
srl h
rr l
rra
srl h
rr l
rra
ld h, l
ld l, a
push hl
pop bc
ld a, b
add a, 0xE0
ld b, a
ret
not_allocated:
pop bc
ld bc, 0
ret


; Given a pid BC, allocate the space for that process and return a pointer to it in BC
; Clobbers A, DE, HL, and IX 
allocate_process:
push bc
ld a, c
and 0x7
sla a
sla a
sla a
or 0xC7
ld (set_ins_2+1), a
; Shift bc right 3 times to get the byte number in c
srl b
rr c
srl b
rr c
srl b
rr c
ld hl, process_allocation_bitmap
add hl, bc
ld a, (hl)
set_ins_2: set 0, a
ld (hl), a
pop bc
push bc
; Shift bc right 6 times to get the page number in c
push bc
pop hl
xor a
add hl, hl
rla
add hl, hl
rla
ld l, h
ld h, a
push hl
pop bc
; Multiply the page number by 3 to get the start offset in the table
add hl, bc
add hl, bc
push hl
pop bc
ld hl, process_heap_pages
add hl, bc
ld a, (hl)
cp 0
jp nz, return_ptr
push hl
call get_free_frame
pop de
ld a, c
ld (de), a
inc de
ld a, l
ld (de), a
inc de
ld a, h
ld (de), a
dec de
dec de
push de
pop hl
return_ptr:
ld c, 0xE
ld b, (hl)
inc hl
ld e, (hl)
inc hl
ld d, (hl)
call set_frame
pop bc
ld a, c
and 0x3F
ld c, a
ld b, 0
; Shift BC left 6 bits (BC=BC*64)
xor a
srl b
rr c
rra
srl b
rr c
rra
ld b, c
ld c, a
ld a, b
add a, 0xE0
ld b, a
ret

; Given a pid in BC, free the processes memory
; Clobber A, BC, and HL
free_process:
ld a, c
and 0x7
sla a
sla a
sla a
or 0x87
ld (res_ins+1), a
; Shift bc right 3 times to get the byte number in c
srl b
rr c
srl b
rr c
srl b
rr c
ld hl, process_allocation_bitmap
add hl, bc
ld a, (hl)
res_ins: res 0, a
ld (hl), a
ret