! Mouse supervisor
! (c) RWT July 1988

%option "-low-nons-half-nodiag-nocheck-nostack-nowarn"
%include "mouse:nmouse.inc-nolist"
@16_ff2003 %byte level 1 interrupt register

%constinteger userbit=128,privbit=64

%routine rommoan(%string(255)s,%integer n)
  rompsym(nl); rompstr(s); rompsym(' '); romphex(n); rompsym(nl)
%end

*temp d0

%systempredicate valid object(%record(*)%name r)
%register(a0)%record(objectfm)%name o
%integer x = addr(objectpool)
  x = a0-x
  %falseunless 0<=x<sizeof(objectpool)
  %falseunless rem(x,sizeof(objectpool(0)))=0
  %falseif o_header_owner==nil
  %true
%end

%systempredicate valid message(%record(messagefm)%name m)
%integer x = addr(messagepool)
  x = a0-x
  %falseunless 0<=x<sizeof(messagepool)
  %falseunless rem(x,sizeof(messagepool(0)))=0
  %falseif m_header_owner==nil
  %true
%end

! Queue handling

! NB only the supervisor may LOCK queues.  All other queue
! operations are available to the user processes.
! Note that many of these corrupt very few registers.

%routine lock(%registerrecord(queue fm)%name x)
! First check to make sure X points at one of the known objects.
! Then attempt (up to 99 times) to lock it.
%label done
%register(d0)%integer i
  *move.l a0,-(sp)
  %if valid object(x) %start
    i = 99
    %cycle
      *tas x_lock; *bpl done  {success ->
      i = i//1                {back off a bit
      i = i-1                 {keep trying
    %repeatuntil i=0
    *temp
    rommoan("*Lock: Timeout ",addr(x))
  %else
    rommoan("*Lock: Invalid Queue ",addr(x))
  %finish
{Even after error, pretend all is well}
done:
  *move.l (sp)+,a0
%end

%routine unlock(%registerrecord(queue fm)%name x)
  *bclr #7,x_lock
%end

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

%systempredicate queue empty (%registerrecord(queue fm)%name q)
! Return TRUE/FALSE if Q is EMPTY/NOT.
! Empty queue descriptors are EXPECTED to be canonical,
! but we tolerate NIL (in which case we MAKE it canonical).
%label t,s
  *move.l q_forward,d0; *beq s  {setup queue(q) %andtrueif q_forward==nil}
  *cmp.l d0,q; *beq t           {%trueif q_forward==q}
  %false
s:setup queue(q)
t:%true
%end

%systemroutine enqueue(%registerrecord(queue fm)%name c,q)
! Add to end
%register(a2)%record(queue fm)%name t
  *exg c,q
    %if queue empty(c{i.e.q}) %start;%finish {force canonical form}
  *exg c,q
  *move.l t,-(sp)
    t == q_backward
    t_forward == c; q_backward == c
    c_forward == q; c_backward == t; c_queue == q
  *move.l (sp)+,t
%end

%routine lenqueue(%registerrecord(queuefm)%name c,q)
! Lock Q, then enqueue C on Q, then unlock Q.
  *exg c,q
    lock(c{i.e.q})
  *exg c,q
  enqueue(c,q)
  *exg c,q
    unlock(c{i.e.q})
  *exg c,q
%end

%systemrecord(*)%map dequeue(%record(queue fm)%name q)
! Remove from front
%record(queue fm)%name c,h
  *temp a0
  %result == nil %if queue empty(q)
  c == q_forward
  h == c_forward
  q_forward == h
  h_backward == q
  setup queue(c)
  %result == c
%end

%record(*)%map ldequeue(%record(queuefm)%name q)
%record(queuefm)%name c
  *temp a0
  lock(q)
    c == dequeue(q)
  unlock(q)
  %result == c
%end

%systemroutine requeue(%registerrecord(queue fm)%name c,q)
! Add to front
%register(a2)%record(queue fm)%name h
  *exg c,q
    %if queue empty(c{i.e.q}) %start;%finish {force canonical form}
  *exg c,q
  *move.l h,-(sp)
    h == q_forward
    h_backward == c; q_forward == c
    c_backward == q; c_forward == h; c_queue == q
  *move.l (sp)+,h
%end

%systemrecord(*)%map unqueue(%record(queue fm)%name q)
! Remove from end
%record(queue fm)%name c,t
  *temp a0
  %result == nil %if queue empty(q)
  c == q_backward
  t == c_backward
  q_backward == t
  t_forward == q
  setup queue(c)
  %result == c
%end

*temp

%systemroutine exqueue(%record(queue fm)%name c)
! Remove from middle
%record(queue fm)%name l,r
  %unless c_queue==nil %start
    l == c_backward; r == c_forward
    l_forward == r; r_backward == l
  %finish
  setup queue(c)
%end

%systemroutine inqueue(%record(queue fm)%name c,l,r)
! Insert C between L and R
  %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
    setup queue(c)
  %finish
%end

%routine suspend(%register(a1{*NB*})%record(queue fm)%name q)
! Suspend the current process by queueing it on Q.
! Alert the local scheduler, then enter an idle loop.
! NB Caller must be in supervisor mode at IPL 7.
  lenqueue(current process_header,q) %unless q==nil
  current process == nil
  level 1 interrupt register = 255
  a7 = 16_3f00-6*64 {{memtop       {switch to idle interrupt stack
  %cycle
    *stop #16_2000
  %repeat
%end

%systemroutine int signal semaphore (%registerrecord(semaphore fm)%name s)
! Callable directly by interrupt handlers, which must not use TRAPs.
! Also called from within supervisor.
%register(a0)%record(queue fm)%name h0
%register(a1)%record(queue fm)%name h1
%register(a1)%record(process fm)%name p
%register(a1)%record(run queue fm)%name q
  %unless valid object(s) %and s_header_tag='S' %start
    *temp
    rommoan("Int Signal: Invalid Semaphore",addr(s))
    %return
  %finish
  *temp 0
  *mfsr d1
  *otsr #16_700
    lock(s_header)
    s_counter = s_counter+1
    %if s_counter>0 %start  {no kicking needed}
      unlock(s_header)
    %else
! Now *very carefully* transfer the process at the head of the
! semaphore wait queue to the tail of that process's run queue.
      h1 == s_header            {A0:S, A1:S
      h0 == dequeue(s_header)   {A0:P, A1:S
      *exg h0,h1                {A0:S, A1:P
        unlock(h0)              {unlock(s_header)
        h0 == p_runqueue_header {A0:Q, A1:P
        lock(h0)
      *exg h0,h1                {A0:P, A1:Q
      enqueue(h0,h1)            {enqueue(p_header,q_header)
      *temp a0
      unlock(q_header)          {A0:Q, A1:Q
      q_tickler = 255           {A0:T, A1:Q
    %finish
  *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

%systempredicate valid semaphore(%record(semaphorefm)%name s)
  %falseunless valid object(s) %and s_header_tag='S'
  %true
%end

%systempredicate valid mailbox(%record(mailbox fm)%name m)
  %falseunless valid object(m) %and m_header_tag='M'
  %falseunless valid semaphore(m_semaphore)
  *move.l m,a0
  %true
%end

%routine check semaphore(%record(semaphorefm)%name s)
  %signal 0,4,addr(s),"Invalid Semaphore" %unless valid semaphore(s)
%end

%routine check mailbox(%record(mailboxfm)%name m)
  %signal 0,4,addr(m),"Invalid Mailbox" %unless valid mailbox(m)
%end

%routine check message(%record(messagefm)%name m)
  %returnif valid message(m) %and queue empty(m_header)
  %signal 0,4,addr(m),"Invalid Message"
%end

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

%systemroutine semaphore wait(%record(semaphore fm)%name s)
  check semaphore(s) %unless s==nil
  *trap #7
%end

%systemrecord(message fm)%map get message buffer {from pool}
  semaphore wait(message semaphore)
%register(a0)%record(*)%name m
  *trap #8
  %result == m
%end

%systemroutine put message buffer {to pool} (%record(message fm)%name m)
  check message(m)
  *trap #9
  signal semaphore(message semaphore)
%end

%systemroutine put message {into mailbox without signalling semaphore} -
  (%record(message fm)%name message, %record(mailbox fm)%name mailbox)
  check mailbox(mailbox)
  check message(message)
  *move.l mailbox,a1
  *trap #10
%end

%systemroutine send message {to mailbox and signal semaphore} -
  (%record(message fm)%name message, %record(mailbox fm)%name mailbox)
  put message buffer(message) %andreturnif mailbox==nil
  put message(message,mailbox)
  signal semaphore(mailbox_semaphore)
%end

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

%systemrecord(message fm)%map receive message {out of mailbox after waiting} -
  (%record(mailbox fm)%name mailbox)
  semaphore wait(mailbox_semaphore)
  %result == get message(mailbox)
%end

%record(*)%map create kernel object(%string(255)id,
   %register(a1)%integer tag,param)
%string(31)dump
  *movem.l d0-d7,dump
  *movem.l id,d0-d7
  *trap #12
  *movem.l dump,d0-d7
%end

%record(*)%map lookup kernel object(%string(255)id,%register(a1)%integer tag)
%string(31)dump
  *movem.l d0-d7,dump
  *movem.l id,d0-d7
  *trap #13
  *movem.l dump,d0-d7
%end

%systemrecord(semaphore fm)%map create semaphore(%string(255)id,%integer c)
%record(*)%name s == create kernel object(id,'S',c)
  %result == s %unless s==nil
  id = "Cannot create semaphore ".id
  %signal 0,4,,id
%end

%systemrecord(semaphore fm)%map lookup semaphore(%string(255)id)
%record(*)%name s == lookup kernel object(id,'S')
  %result == s %unless s==nil
  id = "Cannot find semaphore ".id
  %signal 0,4,,id
%end

%systemroutine delete semaphore(%record(semaphore fm)%name s)
  check semaphore(s)
  *trap #14
%end

%systemrecord(mailbox fm)%map create mailbox -
  (%string(255)id,%record(semaphore fm)%name s)
%record(*)%name m == create kernel object(id,'M',addr(s))
  %result == m %unless m==nil
  id = "Cannot create mailbox ".id
  %signal 0,4,,id
%end

%systemrecord(mailbox fm)%map lookup mailbox(%string(255)id)
%record(*)%name m == lookup kernel object(id,'M')
  %result == m %unless m==nil
  id = "Cannot find mailbox ".id
  %signal 0,4,,id
%end

%systemroutine delete mailbox(%record(mailbox fm)%name m)
  check mailbox(m)
  *trap #14
%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_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_forward
  pc = current interrupt handler_pc
%end

%systemroutine add interrupt handler(%integer level)
%register(a1)%record(queuefm)%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

*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

%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
%integer x=integer(16_1010)
@6(a7) %integer pc
  pc = pc-2
  *rts
%end

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

%routine trap7 {semaphore wait} (%registerrecord(semaphore fm)%name s)
%register(a0)%record(queue fm)%name h0,h1
  *temp d0/a1
  *otsr #16_700
  suspend(nil) %if s==nil              {quiet death}
  lock(s_header)
  s_counter = s_counter-1
  %if s_counter<0 %start               {need to wait}
    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}
      *temp 0
      h1 == s_header
      h0 == currentprocess_header
      enqueue(h0,h1)
      *temp a0/a1
      unlock(h1)
      suspend(nil)
    %finish
    unlock(s_header)                   {otherwise false alarm}
    *movem.l (sp)+,d2-d7/a2-a6         {undo preservation}
    *movem.l (sp)+,d0-d1/a0-a1/a4
    *rte
  %finish
  unlock(s_header)
  *rte
%end

%routine trap8 {get message buffer}
%register(a0)%record(message fm)%name message
%integer i
  *otsr #16_700
  %for i = 1,1,message pool size %cycle
    message == message pool(next message)
    next message = next message-1
    next message = message pool size-1 %if next message<0
    %if message_header_owner==nil %start
      message_header_owner == current process
      *add.l #4,sp; *rte
    %finish
  %repeat
  message == nil
  *add.l #4,sp; *rte
%end

%routine trap9 {put message buffer} (%registerrecord(message fm)%name message)
  message_header_owner == nil
  *rte
%end

%routine trap10 {put message} (%registerrecord(message fm)%name message,
    %registerrecord(mailbox fm)%name mailbox)
  *otsr #16_700
  lenqueue(message_header,mailbox_header)
  *rte
%end

%routine trap11 {get message} (%record(mailbox fm)%name mailbox)
%record(message fm)%name message
  *otsr #16_700
  message == ldequeue(mailbox_header)
  *move.l message,a0
  *lea 8(sp),sp
  *rte
%end

*temp

%routine trap12 {create object} (%register(a1)%integer tag,param)
! NB object name passed in d0-d7
%string(31)id
%record(object fm)%name o
%integer i
  *movem.l d0-d7,id
  *clr.l d4
  i = 0
  length(id) = 31 %if length(id)>31
  i = objectpoolsize %if id=""
  %cycle
    %if i=objectpoolsize %start {not found}
      %for i = 1,1,object pool size %cycle
        o == object pool(next object)
        next object = next object-1
        next object = object pool size-1 %if next object<0
        %if o_header_owner==nil %start
          o_header_owner == current process
          o_name = id
          o_param = param
          o_header_tag = tag
          setup queue(o_header)
          *move.l o,a0
          *lea 40(sp),sp
          *rte
        %finish
      %repeat
      o == nil; %exit {no more objects available}
    %finish
    o == object pool(i)
    %if o_header_owner##nil %and o_name=id %and o_header_tag=tag %start
      %exit     {already there}
    %finish
    i = i+1
  %repeat
  *move.l o,a0
  *lea 40(sp),sp
  *rte
%end

%routine trap13 {lookup object} (%register(a1)%integer tag)
! NB object name passed in d0-d7
%string(31)id
%integer i
%record(objectfm)%name o
  *movem.l d0-d7,id
  *clr.l d4
  i = 0
  length(id) = 31 %if length(id)>31
  i = objectpoolsize %if id=""
  %cycle
    o == nil %andexitif i=objectpoolsize
    o == object pool(i)
    %exitif o_header_owner##nil %and o_header_tag=tag %and o_name=id {found}
    i = i+1
  %repeat
  *move.l o,a0
  *lea 40(sp),sp; *rte
%end

*temp d0-d1/a0-a1

%routine trap14 {delete object} (%registerrecord(object fm)%name object)
  object_header_owner == nil %if object_header_owner==currentprocess
  *rte
%end

%routine int1
  *movem.l d0-d1/a0-a1/a4,-(sp)
  current interrupt handler == intchain(1)_forward
  a0 = current interrupt handler_pc
  *jmp (a0)
%end

%routine int2
  *movem.l d0-d1/a0-a1/a4,-(sp)
  current interrupt handler == intchain(2)_forward
  a0 = current interrupt handler_pc
  *jmp (a0)
%end

%routine int3
  *movem.l d0-d1/a0-a1/a4,-(sp)
  current interrupt handler == intchain(3)_forward
  a0 = current interrupt handler_pc
  *jmp (a0)
%end

%routine int4
  *movem.l d0-d1/a0-a1/a4,-(sp)
  current interrupt handler == intchain(4)_forward
  a0 = current interrupt handler_pc
  *jmp (a0)
%end

%routine int5
  *movem.l d0-d1/a0-a1/a4,-(sp)
  current interrupt handler == intchain(5)_forward
  a0 = current interrupt handler_pc
  *jmp (a0)
%end

%routine int6
  *movem.l d0-d1/a0-a1/a4,-(sp)
  current interrupt handler == intchain(6)_forward
  a0 = current interrupt handler_pc
  *jmp (a0)
%end

%routine int7
  *movem.l d0-d1/a0-a1/a4,-(sp)
  current interrupt handler == intchain(7)_forward
  a0 = current interrupt handler_pc
  *jmp (a0)
%end

%routine scheduling interrupt handler
@16_100002 %byte mmu ud asn,*,mmu up asn
%register(d1)%integer i
  *otsr #16_700
  interrupted priority = -1
  %unless currentprocess==nil %start
    interrupted priority = current process_priority
    %if current process_time slice left<0 %start
      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
      interrupted priority = -1
    %finish
  %finish
  i = 7
  %while i > interrupted priority %cycle
    rivalprocess == ldequeue(run queue(i)_header)
    %unless rivalprocess==nil %start
      %unless current process==nil %start
        *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
      i = currentprocess_vbr
      *=16_4e7b; *=16_1801 {*movec i,vbr
      i = currentprocess_asn
      mmu ud asn = i; mmu up asn = i
      *movem.l (sp)+,d0-d1/a0-a1/a4
      *rte
    %finish
    i = i-1
  %repeat
  *movem.l (sp)+,d0-d1/a0-a1/a4
  *rte
%end {scheduling interrupt handler}

*temp

%begin
%integer i
%label 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.

  *move.l a7,a0
  *lea exception vector(47),a7
  *pea trap14
  *pea trap13
  *pea trap12
  *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 pools of message buffers and kernel objects

  message pool = 0; next message = message pool size-1
  object pool = 0; next object = object pool size-1
  message semaphore == create semaphore("",message pool size)

! Initialise the interrupt handler chains

  %for i = 1,1,7 %cycle
    setup queue(intchain(i)_header); intchain(i)_pc = addr(intret)
  %repeat
  *lea scheduling interrupt handler,a0; intchain(1)_pc = a0

! Create the run queues

  %for i = 0,1,7 %cycle
    runqueue(i) == create kernel object("",'Q',addr(level 1 interrupt register))
  %repeat

! Finally a bit of sly magic to interact with loader

  set priority(7)
  record(memtop-1024) = currentprocess
  %return

intret:
  *movem.l (sp)+,d0-d1/a0-a1/a4
  *rte
%end {setup supervisor}