[3ae31e9] | 1 | * ------------------------------------------------------------------------------
|
---|
| 2 | * smsig.s -- Multi-Tasker -- SM_Sig and SM_Wait for assembly language routines
|
---|
| 3 | * Version 22 -- 1988-04-17 -- D.N. Lynx Crowe
|
---|
| 4 | * (c) Copyright 1988 -- D.N. Lynx Crowe
|
---|
| 5 | * ------------------------------------------------------------------------------
|
---|
| 6 | *
|
---|
| 7 | .text
|
---|
| 8 | *
|
---|
| 9 | .xdef SM_Sig
|
---|
| 10 | .xdef SM_Wait
|
---|
| 11 | *
|
---|
| 12 | .xref _MT_RdyQ
|
---|
| 13 | .xref _MT_CurP
|
---|
| 14 | *
|
---|
| 15 | .xref _MT_LSpc
|
---|
| 16 | .xref _MT_LWpc
|
---|
| 17 | *
|
---|
| 18 | * Register equates
|
---|
| 19 | * ----------------
|
---|
| 20 | RSEM .equ D7 * semaphore contents
|
---|
| 21 | RIPL .equ D6 * saved IPL
|
---|
| 22 | RPRI .equ D5 * task priority
|
---|
| 23 | RTMP .equ D4 * temporary
|
---|
| 24 | *
|
---|
| 25 | RTCP .equ A5 * TCB pointer
|
---|
| 26 | RPRV .equ A4 * previous TCB pointer
|
---|
| 27 | RCUR .equ A3 * current TCB pointer
|
---|
| 28 | RPTR .equ A2 * temporary TCB pointer
|
---|
| 29 | *
|
---|
| 30 | * Miscellaneous equates
|
---|
| 31 | * ---------------------
|
---|
| 32 | IPL7 .equ $0700 * processor IPL 7 mask
|
---|
| 33 | TRAP_NXT .equ 9 * swapper TRAP number 2
|
---|
| 34 | STP_BIT .equ 3 * stop bit number (LS byte of flags)
|
---|
| 35 | *
|
---|
| 36 | * Parameter offsets
|
---|
| 37 | * -----------------
|
---|
| 38 | PSEM .equ 8 * LONG -- pointer to semaphore
|
---|
| 39 | *
|
---|
| 40 | .page
|
---|
| 41 | *
|
---|
| 42 | * TCB definitions
|
---|
| 43 | * ---------------
|
---|
| 44 | NEXT .equ 0 * LONG - next TCB in queue
|
---|
| 45 | FWD .equ NEXT+4 * LONG - next TCB in chain
|
---|
| 46 | TID .equ FWD+4 * WORD - task ID
|
---|
| 47 | PRI .equ TID+2 * WORD - task priority
|
---|
| 48 | SLICE .equ PRI+2 * LONG - slice time limit
|
---|
| 49 | TCB_REG .equ SLICE+4 * LONG[16] - registers
|
---|
| 50 | TCB_SP .equ TCB_REG+64 * LONG - stack pointer
|
---|
| 51 | TCB_PC .equ TCB_SP+4 * LONG - program counter
|
---|
| 52 | TCB_SR .equ TCB_PC+4 * WORD - status register
|
---|
| 53 | FLAGS .equ TCB_SR+2 * WORD - task flags
|
---|
| 54 | TOS .equ FLAGS+2 * LONG - top of stack pointer
|
---|
| 55 | *
|
---|
| 56 | MTF_RDY .equ $0001 * 'ready' bit
|
---|
| 57 | *
|
---|
| 58 | MTF_SWT .equ $0002 * 'wait' bit
|
---|
| 59 | NOT_SWT .equ $FFFD * 'wait' bit complement
|
---|
| 60 | *
|
---|
| 61 | .page
|
---|
| 62 | *
|
---|
| 63 | * SM_Sig -- signal a semaphore
|
---|
| 64 | * ------ ------------------
|
---|
| 65 | SM_Sig: link a6,#0 * link stack frames
|
---|
| 66 | move.l 4(a6),_MT_LSpc * log last PC
|
---|
| 67 | movem.l RTMP-RSEM/RPTR-RTCP,-(sp) * save registers
|
---|
| 68 | move.w sr,RIPL * save interrupt level
|
---|
| 69 | ori.w #IPL7,sr * DISABLE INTERRUPTS
|
---|
| 70 | movea.l PSEM(a6),RPTR * get pointer to semaphore
|
---|
| 71 | *
|
---|
| 72 | L00: move.l (RPTR),RSEM * get semaphore contents
|
---|
| 73 | beq L13 * jump if empty
|
---|
| 74 | *
|
---|
| 75 | btst #0,RSEM * check for waiting tasks
|
---|
| 76 | beq L10 * jump if something waiting
|
---|
| 77 | *
|
---|
| 78 | move.l RSEM,RTMP * check for maximum count
|
---|
| 79 | andi.l #$FFFFFFFE,RTMP * ...
|
---|
| 80 | cmpi.l #$FFFFFFFE,RTMP * ...
|
---|
| 81 | beq L9 * jump if limit hit
|
---|
| 82 | *
|
---|
| 83 | addq.l #2,RSEM * add 1 to semaphore count
|
---|
| 84 | bra L12 * go update semaphore
|
---|
| 85 | *
|
---|
| 86 | L10: movea.l RSEM,RTCP * point at waiting task TCB
|
---|
| 87 | move.l (RTCP),RSEM * update semaphore
|
---|
| 88 | clr.l (RTCP) * clear NEXT of TCB
|
---|
| 89 | andi.w #NOT_SWT,FLAGS(RTCP) * turn off TCB wait bit
|
---|
| 90 | btst.b #STP_BIT,FLAGS+1(RTCP) * check stop bit
|
---|
| 91 | beq L01 * jump if not set
|
---|
| 92 | *
|
---|
| 93 | bclr.b #STP_BIT,FLAGS+1(RTCP) * clear stop bit
|
---|
| 94 | bra L00 * go check for another task
|
---|
| 95 | *
|
---|
| 96 | L01: move.w PRI(RTCP),RPRI * setup to put TCB on ready queue
|
---|
| 97 | lea _MT_RdyQ,RCUR * ...
|
---|
| 98 | *
|
---|
| 99 | L15: movea.l RCUR,RPRV * follow TCB chain
|
---|
| 100 | movea.l (RPRV),RCUR * ...
|
---|
| 101 | move.l RCUR,RTMP * ...
|
---|
| 102 | beq L14 * jump if end of chain
|
---|
| 103 | *
|
---|
| 104 | cmp.w PRI(RCUR),RPRI * is this the place ?
|
---|
| 105 | bls L15 * loop to next one if not
|
---|
| 106 | *
|
---|
| 107 | L14: move.l RTCP,(RPRV) * put TCB on ready queue
|
---|
| 108 | move.l RCUR,(RTCP) * ...
|
---|
| 109 | ori.w #MTF_RDY,FLAGS(RTCP) * set TCB ready bit
|
---|
| 110 | bra L12
|
---|
| 111 | *
|
---|
| 112 | L13: moveq.l #3,RSEM * set semaphore count = 1
|
---|
| 113 | *
|
---|
| 114 | L12: move.l RSEM,(RPTR) * update semaphore
|
---|
| 115 | *
|
---|
| 116 | L9: move.w RIPL,sr * RESTORE INTERRUPTS
|
---|
| 117 | movem.l (sp)+,RTMP-RSEM/RPTR-RTCP * restore registers
|
---|
| 118 | unlk a6 * unlink stack frames
|
---|
| 119 | rts
|
---|
| 120 | *
|
---|
| 121 | .page
|
---|
| 122 | *
|
---|
| 123 | * SM_Wait -- wait on a semaphore
|
---|
| 124 | * ------- -------------------
|
---|
| 125 | SM_Wait: link a6,#0 * link stack frames
|
---|
| 126 | move.l 4(a6),_MT_LWpc * log last PC
|
---|
| 127 | movem.l RTMP-RSEM/RPTR-RCUR,-(sp) * save registers
|
---|
| 128 | move.w sr,RIPL * save interrupt level
|
---|
| 129 | ori.w #IPL7,sr * DISABLE INTERRUPTS
|
---|
| 130 | movea.l PSEM(a6),RPTR * get semaphore pointer
|
---|
| 131 | move.l (RPTR),RSEM * get semaphore contents
|
---|
| 132 | beq L19 * jump if not signalled yet
|
---|
| 133 | *
|
---|
| 134 | btst.l #0,RSEM * see if anything is waiting
|
---|
| 135 | beq L19 * jump if so
|
---|
| 136 | *
|
---|
| 137 | move.l RSEM,RTMP * check semaphore count
|
---|
| 138 | andi.l #$FFFFFFFE,RTMP * ...
|
---|
| 139 | beq L20 * jump if not signalled yet
|
---|
| 140 | *
|
---|
| 141 | subq.l #2,RSEM * decrement semaphore count
|
---|
| 142 | move.l RSEM,(RPTR) * update semaphore
|
---|
| 143 | bra L18 * exit
|
---|
| 144 | *
|
---|
| 145 | L20: clr.l (RPTR) * make SEM a NIL pointer
|
---|
| 146 | *
|
---|
| 147 | L19: movea.l _MT_CurP,RCUR * point at current TCB
|
---|
| 148 | clr.l (RCUR) * clear NEXT of current TCB
|
---|
| 149 | ori.w #MTF_SWT,FLAGS(RCUR) * indicate we're waiting
|
---|
| 150 | *
|
---|
| 151 | L30: tst.l (RPTR) * pointing at SEM queue end ?
|
---|
| 152 | beq L31 * jump if so
|
---|
| 153 | *
|
---|
| 154 | movea.l (RPTR),RPTR * point at next TCB in SEM queue
|
---|
| 155 | bra L30 * keep searching for end of queue
|
---|
| 156 | *
|
---|
| 157 | L31: move.l RCUR,(RPTR) * set NEXT of end of queue
|
---|
| 158 | trap #TRAP_NXT * swap to next ready task
|
---|
| 159 | *
|
---|
| 160 | L18: move.w RIPL,sr * RESTORE INTERRUPTS
|
---|
| 161 | movem.l (sp)+,RTMP-RSEM/RPTR-RCUR * restore registers
|
---|
| 162 | unlk a6 * unlink stack frames
|
---|
| 163 | rts * return to caller
|
---|
| 164 | *
|
---|
| 165 | .end
|
---|