!  File  NMOUSE:SUPER

! Supervisor

%option "-low-nons-nodiag-nocheck-nostack-nowarn"
%include "mouse.inc"

%record(*)%map star(%record(*)%name r); %result == r; %end

@16_ff2003 %byte level 1 interrupt register

! Queue handling

%routine lock(%registerrecord(queue fm)%name x)
! Attempt to lock queue X.
! Report error if we don't succeed after 99 attempts,
! but then carry on as if we had succeeded.
! This routine must only be called from within
! the supervisor, and interrupts must be off.
! D0 is the only register which is changed.
%label done
%register(d0)%integer i
%option "-half"
  *temp 0
  i = 99
  %cycle
    *tas x_lock; *bpl done  {success ->
    i = i//1                {slight delay
    i = i-1                 {keep trying
  %repeatuntil i=0
  *temp d0/a0
  *move.l a0,-(sp)
  rompstr("*Queue lock timeout ")
  *move.l (sp)+,a0
  rompsym(x_tag>>8); rompsym(x_tag)
  rompsym(' '); romphex(addr(x)); rompsym(nl)
  {Despite error pretend all is well}
done:
%end

%routine unlock(%registerrecord(queue fm)%name x)
! Unlock queue X, report error if it wasn't locked.
! No registers changed.
%label ok
  *bclr #7,x_lock; *bne ok
  *temp d0/a0
  *move.l a0,-(sp)
  rompstr("*Queue unlock error ")
  *move.l (sp)+,a0
  rompsym(x_tag>>8); rompsym(x_tag)
  rompsym(' '); romphex(addr(x)); rompsym(nl)
ok:
%end

%systemroutine setup queue (%record(queue fm)%name q)
! Make Q canonically empty.
  q_forward == q; q_backward == q; q_queue == q; q_lock = 0
%end

%systempredicate queue empty (%record(queue fm)%name q)
! Return TRUE/FALSE if Q is EMPTY/NOT.
! We don't bother to check for consistency.
! For example, Q_FORWARD==Q and Q_BACKWARD==Q should always
! be either both true or both false.
{}%if q_forward==nil %start
{}  setup queue(q); %true
{}%finish
  %trueif q_forward==q; %false
%end

%systemroutine enqueue(%record(queue fm)%name c,q)
! Add cell C to end of queue Q
! *No registers changed*
%record(queue fm)%name tail
  *temp a0/a1
  tail == q_backward
{}tail == q %if tail==nil
  c_forward == q; c_backward == tail; c_queue == q
  tail_forward == c; q_backward == c
  *move.l c,a0; *move.l q,a1
%end

%routine lenqueue(%registerrecord(queue fm)%name c,q)
! Enqueue with lock/unlock.
! Registers changed: D0
  *exg c,q; lock(c); *exg c,q
  enqueue(c,q)
  *exg c,q; unlock(c); *exg c,q
%end

%systemrecord(*)%map dequeue(%record(queue fm)%name q)
! Remove a cell from front of queue Q, return NIL if Q empty.
! Registers changed: A1 (and of course A0).
%record(queue fm)%name cell,head
  *temp a0/a1
  cell == q_forward
{}%result == nil %if cell==nil
  %result == nil %if cell==q
  head == cell_forward
  q_forward == head; head_backward == q
  cell_forward == cell; cell_backward == cell; cell_queue == nil
  %result == cell
%end

%record(*)%map ldequeue(%record(queue fm)%name q)
! Dequeue with lock/unlock.
! Registers changed: D0/A0/A1
%record(queue fm)%name c
  *temp a0
  lock(q)
    c == dequeue(q)
  unlock(q)
  %result == c
%end

%systemroutine requeue(%record(queue fm)%name c,q)
! Add to front (as if "putting back" a cell just dequeued).
! Registers changed: A0/A1.
%record(queue fm)%name head
  *temp a0
  head == q_forward
  head_backward == c; q_forward == c
  c_backward == q; c_forward == head; c_queue == q
%end

%systemrecord(*)%map unqueue(%record(queue fm)%name q)
! Remove from end (as if "taking back" a cell just enqueued).
! Registers changed: A0 only.
%record(queue fm)%name cell,tail
  *temp a0
  cell == q_backward; %result == nil %if cell==q
  tail == cell_backward; q_backward == tail
  tail_forward == q
  cell_forward == cell; cell_backward == cell; cell_queue == nil
  %result == cell
%end

%systemroutine exqueue(%record(queue fm)%name c)
! Remove C from wherever in whichever queue it is.
%record(queue fm)%name l,r
  %unless c_queue==nil %start
    l == c_backward; r == c_forward
    l_forward == r; r_backward == l
    c_forward == c; c_backward == c; c_queue == nil
  %finish
%end

%systemroutine inqueue(%record(queue fm)%name c,l,r)
! Insert C into the queue in which L and R are.
! L and R should be adjacent, C is put between them.
  %if l_forward==r %and r_backward==l %and l_queue==r_queue %start
    c_queue == l_queue
    c_forward == r; c_backward == l
    l_forward == c; r_backward == c
  %else
    c_queue == nil
  %finish
%end

%routine suspend(%register(a1)%record(queue fm)%name q)
! Suspend the current process by queueing it on Q.
! If Q==NIL, leave process adrift in space.
! Alert the local scheduler, then enter an idle loop.
! This routine must only be called from within
! the supervisor, and interrupts must be off.
  lenqueue(current process_header,q) %unless q==nil
  current process == nil
  a7 = 16_3f00-6*64   {switch to idle interrupt stack
  level 1 interrupt register = 255
  %cycle
    *stop #16_2000
  %repeat
%end

%systemroutine int signal semaphore (%record(semaphore fm)%name s)
! Callable directly by interrupt handlers, which do not
! have TRAPs at their disposal.  Also called within supervisor.
! Special case: S==NIL means voluntary pre-emption.
! Registers changed: D0/D1/A0/A1.
%record(process fm)%name p
%record(run queue fm)%name q
  *mfsr d1
  *otsr #16_700
  *temp d0/a0-a1
  %if s==nil %start
    suspend(currentprocess_runqueue_header) %unless currentprocess==nil
    %return
  %finish
  %unless s_header_tag='SE' %start
    rompstr("*Signal: Invalid Semaphore "); rompsym(s_header_tag>>8)
    rompsym(s_header_tag); rompsym(' '); romphex(addr(s)); rompsym(nl)
    %return
  %finish
  lock(s_header)
  s_counter = s_counter+1
  %if s_counter>0 %start  {no process transfer needed}
    unlock(s_header)
    *mtsr d1
    %return
  %finish
! Now transfer the process at the head of the semaphore
! wait queue to the tail of that process's run queue.
  p == dequeue(s_header)
  q == p_runqueue
  lock(q_header)
  unlock(s_header)
  enqueue(p_header,q_header)
  %if q_header_forward==p_header %start  {interrupt may be needed}
    unlock(q_header)
    %if q_interrupt register ## level1 interrupt register -
    %or current process==nil -
    %or addr(q)>addr(currentprocess_runqueue) -
    %then q_interrupt register = 255
  %finishelse unlock(q_header)
  *mtsr d1
%end

! Supervisor interface

%systemroutine move to sr(%registerinteger x)
! Loophole: Move X into SR
  *move.l (sp)+,a0
  *trap #0
  *jmp (a0)
%end

%systemintegerfn or to sr(%registerinteger x)
! Loophole: Or X into SR, result: previous SR
  *move.l (sp)+,a0
  *trap #1
  *jmp (a0)
%end

%systemintegerfn cpu time
  *sub.l a0,a0
  *trap #2
%end

%systemintegerfn elapsed time
  *sub.l a0,a0
  *trap #2
  %result = system elapsed time-d1
%end

%systemintegerfn time slice size
  *sub.l a0,a0
  *trap #2
  %result = d2
%end

%systemintegerfn priority
  *sub.l a0,a0
  *trap #2
  %result = d3
%end

%systemintegerfn privilege
  *sub.l a0,a0
  *trap #2
  %result  = d4
%end

%systemroutine set time slice size(%integer ticks)
  *sub.l a0,a0
  *trap #2
  d2 = ticks
  *trap #3
%end

%systemroutine set priority(%integer prio)
  *sub.l a0,a0
  *trap #2
  d3 = prio
  *trap #3
%end

%systemroutine set privilege(%integer onoff)
  *sub.l a0,a0
  *trap #2
  d4 = 0
  d4 = 1 %unless onoff=0
  *trap #3
%end

%systemroutine signal semaphore(%record(semaphore fm)%name s)
  *trap #6
%end

%systemroutine semaphore wait(%record(semaphore fm)%name s)
  *trap #7
%end

%record(*)%map get object(%integer type,val)
%record(semaphore fm)%name o
  o == dequeue(queue pool)
  %unless o==nil %start
    o_header_queue == o
    o_header_tag = type
    o_owner == current process
    o_counter = val
  %finish
  %result == o
%end

%routine put object(%record(queue fm)%name o)
  o_tag = \o_tag
  enqueue(o,queue pool)
%end

%systemroutine put message {into mailbox without signalling semaphore} -
  (%record(message fm)%name m, %record(mailbox fm)%name b)
  *trap #10
%end

%systemrecord(message fm)%map get message {out of mailbox without waiting} -
  (%record(mailbox fm)%name b)
  *trap #11
%end

%systemroutine send message {to mailbox and signal semaphore} -
  (%record(message fm)%name m, %record(mailbox fm)%name b)
  b == buffer pool %if b==nil
  put message(m,b)
  signal semaphore(b_semaphore)
%end

%systemrecord(message fm)%map receive message {out of mailbox after waiting} -
  (%record(mailbox fm)%name b)
  b == buffer pool %if b==nil
  semaphore wait(b_semaphore)
  %result == get message(b)
%end

%systemrecord(message fm)%map acquire message buffer
  %result == receive message(nil)
%end

%systemroutine return message buffer(%record(message fm)%name b)
  send message(b,nil)
%end

%record(semaphore fm)%mapspec create semaphore(%string(255)s,%integer c)
%record(mailbox fm)%mapspec create mailbox(%string(255)s,%record(semaphore fm)%name s)
%routinespec delete semaphore(%record(semaphorefm)%name s)
%routinespec delete mailbox(%record(mailboxfm)%name b)

%systemrecord(message fm)%map exchange message -
  (%record(message fm)%name m,%record(mailbox fm)%name b)
! Send M to B and await reply via one-off temporary mailbox.
%record(semaphore fm)%name s == create semaphore("",0)
%record(mailbox fm)%name r == create mailbox("",s)
  m_reply == r; send message(m,b)
  m == receive message(r)
  delete mailbox(r); delete semaphore(s)
  %result == m
%end

! Interrupt handling

%register(a4)%record(interrupt handler fm)%name current interrupt handler

%systemroutine return from interrupt
@0(a7)%integer pc
  current interrupt handler == current interrupt handler_header_forward
  pc = current interrupt handler_pc
%end

%systemroutine wait for interrupt
@0(a7)%integer pc
  current interrupt handler_pc = pc
  current interrupt handler == current interrupt handler_header_forward
  pc = current interrupt handler_pc
%end

%systemroutine add interrupt handler(%integer level)
%register(a1)%record(queue fm)%name q
%register(d3)%integer sr
  %returnunless 1<=level<=7
  q == int chain(level)_header
  sr = ortosr(16_2700)
  enqueue(current interrupt handler_header,q)
  movetosr(sr)
  *nop {let's not fall foul of the over-eager optimiser - aargh}
%end

%routine register kernel object(%record(*)%name o,%string(255)s)
%string(255)c
{{printstring("""";s;"""=")
  c = cliparam; cliparam = s
  *move.l o,a0; *trap #8; *move.l a0,o
  cliparam = c
{{romphex(addr(o)); newline
  %returnunless o==nil
  s = "Failed to register kernel object ".s
  %signal 0,1,,s
%end

%record(*)%map lookup kernel object(%string(255)s)
%record(*)%name o
%string(255)c
{{printstring("""";s;""":")
  c = cliparam; cliparam = s
  *trap #9; *move.l a0,o
{{romphex(addr(o)); newline
  cliparam = c
  %result == o %unless o==nil
  s = "Failed to locate kernel object ".s
  %signal 0,1,,s
%end

%systemrecord(semaphore fm)%map create semaphore(%string(255)s,%integer n)
%record(semaphore fm)%name q == get object('SE',n)
  register kernel object(q,s) %unless s=""
  %result == q
%end

%systemroutine delete semaphore(%record(semaphore fm)%name q)
  put object(q_header)
  register kernel object(q,"")
%end

%systemrecord(semaphore fm)%map lookup semaphore(%string(255)s)
%record(semaphore fm)%name q == lookup kernel object(s)
  q == nil %unless q_header_tag='SE'
  %result == q
%end

%systemrecord(mailbox fm)%map create mailbox(%string(255)s,%record(semaphore fm)%name x)
%record(mailbox fm)%name q == get object('MB',addr(x))
  register kernel object(q,s) %unless s=""
  %result == q
%end

%systemroutine delete mailbox(%record(mailbox fm)%name q)
  put object(q_header)
  register kernel object(q,"")
%end

%systemrecord(mailbox fm)%map lookup mailbox(%string(255)s)
%record(mailbox fm)%name q == lookup kernel object(s)
  q == nil %unless q_header_tag='MB'
  %result == q
%end

*temp d0-d1/a0-a1

! Implementation of TRAPs
! Although written as routines, none of these actually
! reach their %end, as they have to return using *RTE.

%routine trap0 {move to sr} (%registerinteger v)
@0(a7)%short sr
  *swap v; *move.w sr,v
  *swap v; *move.w v,sr
  *swap v; *ext v
  *rte
%end

%routine trap1 {or to sr} (%registerinteger v)
@0(a7)%short sr
  *swap v; *move.w sr,v
  *swap v; *or.w v,sr
  *swap v; *ext v
  *rte
%end

%constinteger privbit=64

%routine trap2 (%registerrecord(process fm)%name process)
! Obtain process attributes
  process == current process %if process==nil
  d3 = process_priority&7
  d4 = 1; d4 = 0 %if process_asn&privbit=0
  d2 = process_time slice size
  d1 = system elapsed time-process_start time
  d0 = process_cputime
  *rte
%end
  
%routine trap3 (%registerrecord(process fm)%name process,
  %register(d2)%integer time slice size,priority,privilege)
! Change process attributes
  process == current process %if process==nil
  *otsr #16_700
  %if privilege&1=0 %start
    process_asn = process_asn&\privbit
  %else
    process_asn = process_asn!privbit
  %finish
  priority = priority&7
  process_runqueue == runqueue(priority)
  process_priority = priority
  process_time slice size = time slice size
  process_time slice left = time slice size
  *movem.l d0-d1/a0-a1/a4,-(sp)
  *mfusp a4
  *movem.l d2-d7/a2-a6,-(sp)
  current process_ssp = a7
  suspend(current process_runqueue_header)
%end

%routine spare trap
  *subq.l #2,2(sp)
  *move.l 16_1010,-(sp)
  *rts
%end

%routine trap6 {signal semaphore} (%registerrecord(semaphore fm)%name s)
  int signal semaphore(s)
  *rte
%end

%routine trap7 {semaphore wait} (%register(a1)%record(semaphore fm)%name s)
  a1 = a0
  suspend(s_header) %if s==nil           {quiet death}
  %if s_header_tag='SE' %start
    *otsr #16_700
    lock(s_header)
    s_counter = s_counter-1
    %if s_counter>=0 %start              {no need to wait}
      unlock(s_header)
    %else
      s_counter = s_counter+1            {leave consistent while
      unlock(s_header)                   {we preserve context}
      *movem.l d0-d1/a0-a1/a4,-(sp)
      *mfusp a4
      *movem.l d2-d7/a2-a6,-(sp)
      currentprocess_ssp = a7
      lock(s_header)
      s_counter = s_counter-1            {do again}
      %if s_counter<0 %start             {it should be}
        enqueue(currentprocess_header,s_header)
        unlock(s_header)
        s == nil
        suspend(s_header)
      %finish
      unlock(s_header)                   {otherwise false alarm}
{}rompstr("Wait: false alarm"); rompsym(nl)
      *movem.l (sp)+,d2-d7/a2-a6         {undo preservation}
      *movem.l (sp)+,d0-d1/a0-a1/a4
    %finish
  %else
    rompstr("*Wait: Invalid Semaphore "); rompsym(s_header_tag>>8)
    rompsym(s_header_tag); rompsym(' '); romphex(addr(s)); rompsym(nl)
  %finish
  *rte
%end

%routine trap8 {register kernel object} (%registerrecord(kernel object fm)%name o)
  *otsr #16_700
  %if cliparam#"" -
  %and addr(o)&255=0 -
  %and addr(ko(1))<=addr(o)<=addr(ko(queue pool size)) %start
    o_owner == current process
    *temp d0-d1/a1-a2
    *move.l a2,-(sp)
    o_name = cliparam
    *move.l (sp)+,a2
  %finishelse o == nil
  *rte
%end

%routine trap9 {lookup kernel object} (%registerrecord(kernel object fm)%name o)
%register(d1)%integer i
  *otsr #16_700
  %unless cliparam="" %start
    %for i = 1,1,queuepoolsize %cycle
      o == ko(i)
      *temp d0/a1-a2
      %if o_owner##nil %and o_name=cliparam %start
        *rte
      %finish
    %repeat
  %finish
  o == nil
  *rte
%end

%routine trap10 {put message} (%registerrecord(message fm)%name m,
                               %registerrecord(mailbox fm)%name b)
  *otsr #16_700
  lenqueue(m_header,b_header)
  *rte
%end

%routine trap11 {get message} (%registerrecord(mailbox fm)%name b)
  *otsr #16_700
  b == ldequeue(b_header)
  *rte
%end

*temp

%begin
%integer j
%label scheduler,int1,int2,int3,int4,int5,int6,int7,intret
@16_1000 %integerarray exception vector(0:47)

{--------------------------}
{ Main program begins HERE }
{--------------------------}

! Plug in exception vector table entries,
! first traps 14:0, then interrupts 7:1.
! (Trap 15 and Int 0 are dealt with by PREFIX).

  *move.l a7,a0
  *lea exception vector(47),a7
  *pea sparetrap
  *pea sparetrap
  *pea sparetrap
  *pea trap11
  *pea trap10
  *pea trap9
  *pea trap8
  *pea trap7
  *pea trap6
  *pea sparetrap
  *pea sparetrap
  *pea trap3
  *pea trap2
  *pea trap1
  *pea trap0
  *pea int7
  *pea int6
  *pea int5
  *pea int4
  *pea int3
  *pea int2
  *pea int1
  *move.l a0,a7

! Initialise kernel object pools

  queue pool == star(ko(1))
  setup queue(queue pool)
  queue pool_tag = 'QP'

  buffer pool == star(ko(2))
  setup queue(buffer pool_header)
  buffer pool_header_tag = 'BP'

  buffer pool_semaphore == star(ko(3))
  setup queue(buffer pool_semaphore_header)
  buffer pool_semaphore_header_tag = 'SE'
  buffer pool_semaphore_counter = 0

  enqueue(star(ko(j)),queue pool) %for j = 4,1,queue pool size
  return message buffer(star(bu(j))) %for j = 1,1,buffer pool size

! Initialise the interrupt handler chains

  %for j = 1,1,7 %cycle
    setup queue(intchain(j)_header); intchain(j)_pc = addr(intret)
  %repeat
  intchain(1)_pc = addr(scheduler)

! Create the run queues

  %for j = 0,1,7 %cycle
    runqueue(j) == get object('RQ',addr(level 1 interrupt register))
  %repeat

! Return to loader

  set priority(7)
  %stop

! Scheduling interrupt handler

@16_100002 %byte mmu ud asn,*,mmu up asn
%register(d1)%integer i
scheduler:
  *otsr #16_700
  %unless currentprocess==nil %start
    interrupted priority = current process_priority
    %if current process_time slice left<0 %start  {expired}
{{rompstr("Time-slice"); rompsym(nl)
      currentprocess_timesliceleft = currentprocess_time slice size
      *mfusp a4
      *movem.l d2-d7/a2-a6,-(sp)
      current process_ssp = a7
      lenqueue(currentprocess_header,currentprocess_runqueue_header)
      currentprocess == nil
    %finish
  %finish
  interrupted priority = -1 %if currentprocess==nil
  i = 7
  %while i > interrupted priority %cycle
    rivalprocess == ldequeue(run queue(i)_header)
    %unless rivalprocess==nil %start
      %unless current process==nil %start
{}rompstr("Pre-empt ";currentprocess_name;" @"); rompsym(i+'0';nl)
        *mfusp a4
        *movem.l d2-d7/a2-a6,-(sp)
        current process_ssp = a7
        lenqueue(current process_header,current process_run queue_header)
      %finish
      currentprocess == rivalprocess
      currentprocess_starttime = system elapsed time
      a7 = currentprocess_ssp
      *movem.l (sp)+,d2-d7/a2-a6
      *mtusp a4
      %unless cputype=68000 %start
!!      i = currentprocess_vbr
!!      *=16_4e7b; *=16_1801 {*movec i,vbr
        i = currentprocess_asn
        mmu ud asn = i; mmu up asn = i
      %finish
      *movem.l (sp)+,d0-d1/a0-a1/a4
      *rte
    %finish
    i = i-1
  %repeat
intret:
  *movem.l (sp)+,d0-d1/a0-a1/a4
  *rte

int1:
  *movem.l d0-d1/a0-a1/a4,-(sp)
  current interrupt handler == intchain(1)_header_forward
  a0 = current interrupt handler_pc
  *jmp (a0)
int2:
  *movem.l d0-d1/a0-a1/a4,-(sp)
  current interrupt handler == intchain(2)_header_forward
  a0 = current interrupt handler_pc
  *jmp (a0)
int3:
  *movem.l d0-d1/a0-a1/a4,-(sp)
  current interrupt handler == intchain(3)_header_forward
  a0 = current interrupt handler_pc
  *jmp (a0)
int4:
  *movem.l d0-d1/a0-a1/a4,-(sp)
  current interrupt handler == intchain(4)_header_forward
  a0 = current interrupt handler_pc
  *jmp (a0)
int5:
  *movem.l d0-d1/a0-a1/a4,-(sp)
  current interrupt handler == intchain(5)_header_forward
  a0 = current interrupt handler_pc
  *jmp (a0)
int6:
  *movem.l d0-d1/a0-a1/a4,-(sp)
  current interrupt handler == intchain(6)_header_forward
  a0 = current interrupt handler_pc
  *jmp (a0)
int7:
  *movem.l d0-d1/a0-a1/a4,-(sp)
  current interrupt handler == intchain(7)_header_forward
  a0 = current interrupt handler_pc
  *jmp (a0)

%end