clrso
task.pid: so.w 2
task.stack_frame: so.l 1
task.stack_ptr: so.l 1
task.next_ptr: so.l 1
task.address_space: so.l 4
task.secondary_address_space: so.l 4
task.sizeof equ __SO

  include term.i
  include vmem.i
  include pmem.i
  include memory.i
  section .text,text
; Initializes tasking, takes the kernel stack frame in a0
  public tasking_init
tasking_init:
  move.l a0, -(a7)
  move.l #task.sizeof, d0 ; Allocate space for the task data
  jsr malloc
ti_malloc_done:
  move.l (a7)+, a1
  move.w #0, (task.pid,a0)
  move.l a1, (task.stack_frame,a0)
  move.l #0, (task.stack_ptr,a0)
  move.l #0, (task.next_ptr,a0)
  move.l a0, current_process
  move.l #15, d1 ; Put the address space size - 1 in d1
  lea.l (task.address_space,a0), a1 ; Put the address of the task address space in a1
  move.l #kernel_address_space, a0 ; Put the address of the kernel address space in a0
.1:
  move.b (a0)+,(a1)+
  dbra d1, .1 ; Loop back if there is more to copy
  move.w #1, next_pid
  rts

; Creates a new process
; Pointer to address space in a0
; Start address in a1
  public tasking_new_process
tasking_new_process: 
  movem.l a2/a3, -(a7)
  movem.l a0/a1, -(a7)
  move.l #task.sizeof, d0 ; Allocate space for the task data
  jsr malloc
tnp_malloc_done:
  move.l #0, (task.secondary_address_space,a0)
  move.l #0, (task.secondary_address_space+4,a0)
  move.l #0, (task.secondary_address_space+8,a0)
  move.l #0, (task.secondary_address_space+12,a0)
  movem.l (a7)+, a1/a2
  move.w next_pid, d0 ; Get the next free PID
  move.w d0, (task.pid,a0)
  addi.w #1, d0 ; Increment the counter by 1
  move.w d0, next_pid
  movem.l a0/a1, -(a7)
  jsr pmem_pop_frame ; Get a frame for the process' kernel stack
  movem.l (a7)+, a0/a1
  move.l d0, (task.stack_frame,a0)
  ; Map the new process' stack into memory
  movem.l a0/a1/d2/d3, -(a7)
  move.l #1, d1
  move.l #0, d2
  move.l #$2, d3
  jsr vmem_map_free_to
  move.l a0, a3
  movem.l (a7)+, a0/a1/d2/d3
  adda.l #$1000, a3
  ; Push start address onto new process stack
  move.l a2, -(a3)
  ; Push address of user mode switch shim onto process stack
  move.l #tasking_um_switch_shim, -(a3)
  ; Push dummy register values onto new process stack
  move.l #0, -(a3) 
  move.l #0, -(a3)
  move.l #0, -(a3)
  move.l #0, -(a3)

  move.l #0, -(a3)
  move.l #0, -(a3)
  move.l #0, -(a3)
  move.l #0, -(a3)

  move.l #0, -(a3)
  move.l #0, -(a3)
  move.l #0, -(a3)
  ; Set the page number of the new process's kernel stack address to $FEF
  move.l a3, d0
  andi.l #$FFF, d0
  ori.l #$FEF000, d0
  move.l d0, (task.stack_ptr,a0)
  move.l #0, (task.next_ptr,a0)
  ; Unmap the temporary stack mapping
  movem.l a0/a1, -(a7)
  move.l a3, d0 ; Put the page number of the temporary page into a0
  and.l #~($FFF), d0
  move.l d0, a0
  move.l #1, d0
  move.l #0, d1
  jsr vmem_unmap
  movem.l (a7)+, a0/a1
  move.l #15, d1 ; Put the address space size - 1 in d1
tnp_as_copy:
  lea.l (task.address_space,a0), a2 ; Put the address of the task address space in a2
.1:
  move.b (a1)+, (a2)+
  dbra d1, .1 ; Loop back if there is more to copy
  jsr rtr_push_head
  movem.l (a7)+, a2/a3
  rts

tasking_um_switch_shim:
  move.l (a7)+, a0 ; Get the target PC into a0
  ; Push a dummy format/vector word
  move.w #0, -(a7)
  ; Push the PC
  move.l a0, -(a7)
  move.w sr, d0 ; Push the SR with the supervisor bit cleared to make a "fake" exception frame
  andi.w #(~$2000), d0
  move.w d0, -(a7)
  rte ; Switch the CPU into usermode and start executing the new process

; Yields to the next process
  public tasking_yield
tasking_yield:
  movem.l d2-d7/a2-a6, -(a7)
  ; Save the current kernel stack pointer
  move.l current_process, a0
  move.l a7, (task.stack_ptr,a0)
  jsr rtr_pop ; Get the next ready process
  ; Return if there is none
  cmp.l #0, a0
  bne.b .1
  movem.l (a7)+, d2-d7/a2-a6
  rts
.1:
  move.l a0, a2 ; Save the next process in a2
  ; Push the current process on the ready to run list
  move.l current_process, a0
  jsr rtr_push_tail
  ; Update the current process to be the next process to run
  move.l a2, current_process
  ; Activate the next process' address space
  lea.l (task.address_space,a2), a0
  jsr vmem_activate_addr_space
  ; Set the kernel stack frame to the next process' stack frame
  move.l #0, d0
  move.l #$FEF000, a0
  jsr vmem_get_map_ptr
  move.l (task.stack_frame,a2), d0
  ori.l #$3, d0
  move.l d0, (a0)
  move.l vmem_mmu_base_addr, a1
  move.l #$FEF000, ($10,a1)
  move.l (task.stack_ptr,a2), a7
  movem.l (a7)+, d2-d7/a2-a6
  rts

; Exits the current process
  public tasking_exit
tasking_exit:
  movem.l d2-d7/a2-a6, -(a7)
  jsr rtr_pop ; Get the next ready process
  ; Return if there is none
  cmp.l #0, a0
  bne.b .1
  move.l #last_proc_exited_str, a0
  jsr term_println
  stop #$2700
.1:
  move.l a0, a2 ; Save the next process in a2
  ; Update the current process to be the next process to run
  move.l a2, current_process
  ; Activate the next process' address space
  lea.l (task.address_space,a2), a0
  jsr vmem_activate_addr_space
  ; Set the kernel stack frame to the next process' stack frame  
  move.l #0, d0
  move.l #$FEF000, a0
  jsr vmem_get_map_ptr
  move.l (task.stack_frame,a2), d0
  ori.l #$3, d0
  move.l d0, (a0)
  move.l vmem_mmu_base_addr, a1
  move.l #$FEF000, ($10,a1)
  move.l (task.stack_ptr,a2), a7
  movem.l (a7)+, d2-d7/a2-a6
  rts

; Gets the secondary address space of the current process in a0
  public tasking_get_secondary_addr_space
tasking_get_secondary_addr_space:
  move.l current_process, a0
  lea.l (task.secondary_address_space,a0), a0
  rts

; Push the process pointed to by a0 onto the head of the ready to run list
rtr_push_head:
  cmp.l #0, ready_to_run_head
  bne.b .1
  ; Set the ready to run list to the passed-in process if it is empty
  move.l a0, ready_to_run_head
  move.l a0, ready_to_run_tail
  rts
.1:
  move.l ready_to_run_head, (task.next_ptr,a0) ; Set the next pointer of the passed-in process to the current head
  move.l a0, ready_to_run_head ; Set the head to the passed-in process
  rts

; Push the process pointed to by a0 onto the tail of the ready to run list
rtr_push_tail:
  cmp.l #0, ready_to_run_head
  bne.b .1
  ; Set the ready to run list to the passed-in process if it is empty
  move.l a0, ready_to_run_head
  move.l a0, ready_to_run_tail
  rts
.1:
  ; Set the next pointer of the tail to the passed-in process
  move.l ready_to_run_tail, a1
  move.l a0, (task.next_ptr,a1)
  move.l #0, (task.next_ptr,a0) ; Set the next pointer of the passed-in process to NULL
  move.l a0, ready_to_run_tail ; Set the tail to the passed-in process
  rts

; Pop a process of the head of the ready to run list
; Address returned in a0, or 0 if no process
rtr_pop:
  ; Return 0 if the list is empty
  cmp.l #0, ready_to_run_head
  bne.b .1
  move.l #0, a0
  rts
.1:
  ; Set the head to the current head's next pointer
  move.l ready_to_run_head, a0
  move.l (task.next_ptr,a0), ready_to_run_head
  ; If the new head is NULL, set the tail to NULL
  cmp.l #0, ready_to_run_head
  bne.b .2
  move.l #0, ready_to_run_tail
.2:
  rts

  section .bss,bss
ready_to_run_head: ds.b 4
ready_to_run_tail: ds.b 4
current_process: ds.b 4
next_pid: ds.b 2

  section .data,data
last_proc_exited_str: dc.b "Last process exited, halting",0