; Uses a 8k page of memory at $FE0000 to $FE1FFF to store 2047 semaphores
; Semaphores are 16 bit vars. The memory actually has a hidden 17th bit
; which is the reservation flag. The reservation flag is set by loading
; from a semaphore. The reservation flag is cleared by storing to the
; semaphore. IF the reservation bit is clear while attempting a store to
; a semaphore, the store fails and the semaphore is not updated.
; The last word of the memory ($FE0FFE) indicates if the last store to a
; semaphore was successful.
; Memory addresses from $FE1000 to $FE1FFF reflect the raw value in the
; semaphore and are an alias for addresses $FE0000 to $FE0FFF. However
; accessing $FE1xxx with a store is always successful and access with a
; load does not set a reservation.
;
; Lock semaphore
; .X = semaphore to lock
; .Y = #retries must be > 0
;
SEMABASE      = $FE0000
STORESUCCESS   = $FE0FFE

LockSemaphore:
.retry:
   DEY
   BMI   .fail
   LDA SEMABASE,X      ; load and reserve
   BEQ   .0002         ; branch if semaphore not set
   CMP TasknoVar      ; semaphore was set, see if by same task
   BEQ   .0003         ; branch if same task
.0002:
   LDA TasknoVar
   STA SEMABASE,X      ; conditional store and clear reservation
   LDA STORESUCCESS   ; get successful store bit
   BEQ .retry         ; branch if store failed
.0003:
   LDA   #1            ; sucessfully locked semaphore
   RTS
.fail:               ; failed to lock semaphore
   LDA   #0
   RTS


; Unlock semaphore
; .X = semaphore to unlock
;
UnlockSemaphore:
   LDA   #$0000         ; set semaphore to zero
   STA SEMABASE,X      ; this will clear any reservation
   RTS

UnlockAllSemaphores:
   LDA   #$0000
   TAX
.loop:
   STA   SEMABASE+$1000,X   ; access the raw values, store never fails
   INX
   INX
   CPX   #$0FFE
   BNE .loop
   RTS