NewIRQs

; Copyright 1996 Acorn Computers Ltd
;
; Licensed under the Apache License, Version 2.0 (the "License");
; you may not use this file except in compliance with the License.
; You may obtain a copy of the License at
;
;     http://www.apache.org/licenses/LICENSE-2.0
;
; Unless required by applicable law or agreed to in writing, software
; distributed under the License is distributed on an "AS IS" BASIS,
; WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
; See the License for the specific language governing permissions and
; limitations under the License.
;
        TTL     => NewIRQs

; *****************************************************************************
;
; Main IRQ routine:
;     Push  workregs,lr
;     IRQsema -> TOS
;     stack -> IRQsema
;     call IRQ1V
;     IRQs off
;     TOS ->IRQsema
;     process callback, pulling workregs,pc at some point
;
; *****************************************************************************

        ALIGN 32

Initial_IRQ_Code ROUT
        SUB     lr, lr, #4
        Push    "r0, lr"
        MRS     lr, SPSR
        Push    "r1-r3, r11, r12, lr"
; ** For Pete's sake remember to change the heap manager if you change the above
; ** register list!!!!!!! And the [sp_irq, #4*5] below

  [ :LNOT:No26bitCode
        BIC     r0, lr, #&1F            ; clear out foreground mode bits
        ORR     r0, r0, #I32_bit + IRQ26_mode ; force IRQ_26 mode and I bit set
        MSR     CPSR_c, r0
  ]

        LDR     r12, =ZeroPage
        LDR     r0, [r12, #IRQsema]
        Push    r0
        STR     sp_irq, [r12, #IRQsema]
        MOV     lr, pc
        LDR     pc, [r12, #IRQ1V]

; IRQ1V called with r0-r3,r11,r12 trashable. r12=0

; Stu has a theory that 1N cycle can be saved by the default IRQ1V pointing
; at a location containing a branch to our code; we then do something like
;  LDR R0, [R12, #IRQ1V]
;  CMP R0, #OurIRQ1V
;  BNE somebodysonIRQ1V
;  .... fall into default IRQ1V code

        Push     "r10"
        MOV      r10, #UnthreadV
        BL       CallVector
        Pull     "r10"

        LDR      r11, =ZeroPage
        Pull     r0
        STR      r0, [r11, #IRQsema]

  [ :LNOT:No26bitCode
        MRS      r0, CPSR
        ORR      r0, r0, #&10
        MSR      CPSR_c, r0                     ; switch back to IRQ32 mode
  ]

        LDRB     r11, [r11, #CallBack_Flag]
        TEQ      r11, #0
        Pull     "r1-r3, r11, r12, lr", EQ
        MSREQ    SPSR_cxsf, lr
        Pull     "r0, pc", EQ, ^

        TST      r11, #CBack_Postpone
        LDREQ    lr, [sp_irq, #4*5]     ; get SPSR off stack
        TSTEQ    lr, #I32_bit :OR: &0F  ; check we came from USR26 or USR32 mode, with IRQs enabled
        Pull     "r1-r3, r11, r12, lr", NE
        MSRNE    SPSR_cxsf, lr
        Pull     "r0, pc", NE, ^

; Do a CallBack: asked for, not postponed, and we're returning into USR26/32 mode.

        ASSERT   IRQ32_mode :AND: SVC32_mode = IRQ32_mode ; so the following dodgy ops work

        Pull     "r1-r3, r11, r12"
        MRS      r0, CPSR
        ORR      r0, r0, #SVC32_mode
        MSR      CPSR_c, r0
        Push     "r10-r12"                               ; push r10-r12 onto the SVC stack
        BIC      r0, r0, #IRQ32_mode :EOR: SVC32_mode
        MSR      CPSR_c, r0
        Pull     "r10-r12"                      ; SPSR, R0, LR really
 [ No26bitCode :LOR: FixCallBacks
        ORR      r0, r0, #SVC32_mode
 |
        BIC      r0, r0, #&1F
        ORR      r0, r0, #SVC26_mode
 ]
        MSR      CPSR_c, r0
        Push     r12                            ; Save the return address
        MOV      r14, r10                       ; SPSR into R14
        MOV      r0, r11                        ; restore original R0
        LDR      r10, =ZeroPage
        LDRB     r11, [r10, #CallBack_Flag]
      [ FixCallBacks
        B        Do_CallBack_postpone_already_clear
      |
        B        Do_CallBack
      ]

        LTORG

; +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
; Default IRQ1V: despatch on interrupting device

; Now copied to RAM, together with vector entries and device tables

                  ^ 0
 [ HAL
IRQDesp_Link      # 4
IRQDesp_R12Val    # 4
IRQDesp_CallAddr  # 4

IRQDesp_Link_Unshared * 1       ; flag in Link (for _this_ node)
 |
IRQDesp_R12Val    # 4
IRQDesp_CallAddr  # 4
IRQDesp_Link      # 4
 ]
       ASSERT   IRQDesp_CallAddr = IRQDesp_R12Val + 4

       ALIGN 32

DefaultIRQ1Vcode ROUT

 [ HAL
        Push    "r9,lr"
      [ ZeroPage = 0
        MOV     r9, #0
      |
        LDR     r9, %FT02
      ]
        AddressHAL r9                   ; modifies r9
;        MOV     r11, r14                ; r11 trashable
        CallHAL HAL_IRQSource
        Pull    "r9"
        ADR     r2, Devices
        ADD     r1, r0, r0, LSL #1      ; multiply by 3
;        MOV     r14, r11
        ADD     r11, r2, r1, LSL #2     ; so table contains DevNo * 3
 ;       ASSERT  IRQDesp_R12Val = 4
 ;       LDMIB   r1, {r12, pc}
01      MOV     lr, pc
        LDMIA   r11, {r11, r12, pc}
        TST     r11, #IRQDesp_Link_Unshared
        BEQ     %BT01
        Pull    pc
      [ ZeroPage <> 0
02
        DCD     ZeroPage
      ]
 |
        MOV     r3, #IOC                ; base for IOC and IOMD
        LDRB    r0, [r3, #IOMD_DMAREQ]
        TEQ     r0, #0
        ADRNE   r1, IrqDMADevnos
        LDREQB  r0, [r3, #IOCIRQREQB]   ; if not DMA then assume IRQB until we know otherwise
        ADREQ   r1, IrqReqBDevnos

 [ MorrisSupport
;>>>RCM Says should we use separate Morris specific code, cos Morris doesn't support
;>>>RCM all IOMD_DMAREQ bits and non Morris machines don't have IOMD_IRQRQD.
;>>>RCM Look at use of NoInterrupt.
        ADREQ   r12, DeviceTables       ; can't reach these tables with ADR

        TEQEQ   r0, #0
        LDREQB  r0, [r3, #IOMD_IRQRQD]
        ADDEQ   r1, r12, #IrqReqDDevnos-DeviceTables

        TEQEQ   r0, #0
        LDREQB  r0, [r3, #IOMD_IRQRQC]
        ADDEQ   r1, r12, #IrqReqCDevnos-DeviceTables
 ] ; MorrisSupport

        TEQEQ   r0, #0
        LDREQB  r0, [r3, #IOCIRQREQA]   ; not DMA and not IRQB so assume IRQA
        ADREQ   r1, IrqReqADevnos

        LDRB    r0, [r1, r0]            ; pick up offset in device despatcher
        ADD     r1, pc, r0, LSL #2      ; so table contains DevNo * 3
        ASSERT  IRQDesp_R12Val = 0
        LDMIA   r1, {r12, pc}
 ] ; HAL

; ******* IRQ device handlers entered with r0-r3,r11,r12,r14 trashable *******
;   r0  =  device number (if HAL)
;   r3  -> IOC (not in HAL world, unless IOMD HAL is helpful)
;   r12 =  what they asked for
;   r14 =  return address to MOS IRQ exit sequence

 [ HAL

DefaultIRQ1Vcode_end

Devices * DefaultIRQ1Vcode_end + 12

NoInterrupt     * -1
  [ M_CortexA9
MaxInterrupts   * 160
  |
MaxInterrupts   * 96
  ] ; M_CortexA9

DevicesEnd * Devices + MaxInterrupts * 12

        ASSERT  DevicesEnd - DefaultIRQ1Vcode <= DefIRQ1Vspace

 | ; HAL
  [ MorrisSupport
NoInterrupt * 38 ; Morris has IOMD's extra interrupts plus 16 of its own
  |
NoInterrupt * 22 ; IOMD has 6 more interrupts for DMA
  ]

Devices

; Register A devices
; pbusy handler
        & 0             ; R12 value
        & IRQ           ; call address
        & 0             ; link
; ringing handler
        & 0
        & IRQ
        & 0
; printer acknowledge
        & 0
        & IRQ
        & 0
; vsync handler
        & OsbyteVars
        & VsyncIRQ
        & 0
; power on reset: this can't happen, but call IRQ2V if it does.
        & 0
        & IRQ
        & 0
; timer0
        & OsbyteVars
        & TickOne
        & 0
; timer1
        & 0
        & IRQ
        & 0
; FIQ downgrade
        & 0
        & IRQ
        & 0

; register B devices
; PFIQ downgrade
        & PFIQasIRQ_Chain - (PodDesp_Link-PodDesp_R12Val)
        & PFIQasIRQ_Despatch
        & 0
; sound
        & 0
        & IRQ
        & 0
; serial
        & 0
        & IRQ
        & 0
; winnie IRQ
        & 0
        & IRQ
        & 0
; Disc changed
        & 0
        & IRQ
        & 0
; podule IRQ
        & PIRQ_Chain - (PodDesp_Link-PodDesp_R12Val)
        & PIRQ_Despatch
        & 0
; serial TX     (Keyboard serial transmit register empty)
        & IOC
 [ Keyboard_Type = "A1A500"
        & IrqTx
 |
        & IRQ
 ]
        & 0
; serial RX     (Keyboard serial receive register full)
        & IOC
 [ Keyboard_Type = "A1A500"
        & IrqRx
 |
        & IRQ
 ]
        & 0

; IOMD DMA devices
; DMA channel 0
        & 0
        & IRQ
        & 0
; DMA channel 1
        & 0
        & IRQ
        & 0
; DMA channel 2
        & 0
        & IRQ
        & 0
; DMA channel 3
        & 0
        & IRQ
        & 0
; Sound DMA channel 0
        & 0
        & IRQ
        & 0
; Sound DMA channel 1
        & 0
        & IRQ
        & 0

 [ MorrisSupport
; register D devices
; Mouse port Rx full
        & IOC
        & IRQ
        & 0
; Mouse port Tx empty
        & IOC
        & IRQ
        & 0
; AtoD (Joystick)
        & 0
        & IRQ
        & 0
; Nevent1
        & 0
        & IRQ
        & 0
; Nevent2
        & 0
        & IRQ
        & 0
; The following are just place fillers in case IRQD bits 5 to 7 are ever used.
; NoInterrupt
        & 0
        & IRQ
        & 0
; NoInterrupt
        & 0
        & IRQ
        & 0
; NoInterrupt
        & 0
        & IRQ
        & 0

; register C devices
; Bit0
        & 0
        & IRQ
        & 0
; Bit1
        & 0
        & IRQ
        & 0
; Bit2
        & 0
        & IRQ
        & 0
; Bit3
        & 0
        & IRQ
        & 0
; Bit4
        & 0
        & IRQ
        & 0
; Bit5
        & 0
        & IRQ
        & 0
; Bit6
        & 0
        & IRQ
        & 0
; Bit7
        & 0
        & IRQ
        & 0
 ]

; Neither A or B is interrupting, which is impossible: just call IRQ2V anyway
        & 0
        & IRQ
        & 0

DevicesEnd

; Following tables encode the priority of the devices within each register
;
DeviceTables

; Prioritised IOMD DMA device numbers

IrqDMAPrio0     * 1:SHL:5
IrqDMADev0      * IOMD_DMASound1_DevNo

IrqDMAPrio1     * 1:SHL:4
IrqDMADev1      * IOMD_DMASound0_DevNo

IrqDMAPrio2     * 1:SHL:3
IrqDMADev2      * IOMD_DMAChannel3_DevNo

IrqDMAPrio3     * 1:SHL:2
IrqDMADev3      * IOMD_DMAChannel2_DevNo

IrqDMAPrio4     * 1:SHL:1
IrqDMADev4      * IOMD_DMAChannel1_DevNo

IrqDMAPrio5     * 1:SHL:0
IrqDMADev5      * IOMD_DMAChannel0_DevNo


       GBLA  DTabC
DTabC  SETA 1

IrqDMADevnos
  = NoInterrupt*3

; Top 2 bits are always 0 so table need only be 64 bytes
  WHILE DTabC <64
  [ (DTabC:AND:IrqDMAPrio5)<>0
  = IrqDMADev5*3
  |
  [ (DTabC:AND:IrqDMAPrio4)<>0
  = IrqDMADev4*3
  |
  [ (DTabC:AND:IrqDMAPrio3)<>0
  = IrqDMADev3*3
  |
  [ (DTabC:AND:IrqDMAPrio2)<>0
  = IrqDMADev2*3
  |
  [ (DTabC:AND:IrqDMAPrio1)<>0
  = IrqDMADev1*3
  |
  [ (DTabC:AND:IrqDMAPrio0)<>0
  = IrqDMADev0*3
  ]
  ]
  ]
  ]
  ]
  ]
DTabC SETA DTabC+1
  WEND

; generic IRQA bits
IrqReqAPrio0 * por_bit
IrqReqADev0  * PowerOn_DevNo

IrqReqAPrio4 * timer1_bit
IrqReqADev4 * Timer1_DevNo

IrqReqAPrio5 * vsync_bit
IrqReqADev5 * VSync_DevNo

IrqReqAPrio6 * timer0_bit
IrqReqADev6 * Timer0_DevNo

IrqReqAPrio7 * force_bit
IrqReqADev7 * FIQDowngrade_DevNo

; Machine specific IRQB bits (devices 0-2)


IrqReqAPrio1 * 1:SHL:1          ; not used
IrqReqADev1 * 1

 [ ReassignedIOMDInterrupts
 ASSERT IOMDr_PrinterIRQ_DevNo = 2

IrqReqAPrio2 * IOMDr_printer_IRQ_bit
IrqReqADev2 * IOMDr_PrinterIRQ_DevNo

IrqReqAPrio3 * 1:SHL:0          ; not used
IrqReqADev3 * 0
 |
 ASSERT IOMD_PrinterIRQ_DevNo = 0
 ASSERT IOMD_FloppyIndex_DevNo = 2

IrqReqAPrio2 * IOMD_printer_IRQ_bit
IrqReqADev2 * IOMD_PrinterIRQ_DevNo

IrqReqAPrio3 * IOMD_floppy_index_bit
IrqReqADev3 * IOMD_FloppyIndex_DevNo
 ]


DTabC  SETA 1

IrqReqADevnos
  =  NoInterrupt*3
  WHILE DTabC <256
  [ (DTabC:AND:IrqReqAPrio7)<>0
  = IrqReqADev7*3
  |
  [ (DTabC:AND:IrqReqAPrio6)<>0
  = IrqReqADev6*3
  |
  [ (DTabC:AND:IrqReqAPrio5)<>0
  = IrqReqADev5*3
  |
  [ (DTabC:AND:IrqReqAPrio4)<>0
  = IrqReqADev4*3
  |
  [ (DTabC:AND:IrqReqAPrio3)<>0
  = IrqReqADev3*3
  |
  [ (DTabC:AND:IrqReqAPrio2)<>0
  = IrqReqADev2*3
  |
  [ (DTabC:AND:IrqReqAPrio1)<>0
  = IrqReqADev1*3
  |
  [ (DTabC:AND:IrqReqAPrio0)<>0
  = IrqReqADev0*3
  ]
  ]
  ]
  ]
  ]
  ]
  ]
  ]
DTabC SETA DTabC+1
  WEND


; generic IRQB bits
IrqReqBPrio2 * podule_FIQ_as_IRQ_bit
IrqReqBDev2  * PFIQasIRQ_DevNo

IrqReqBPrio3 * serial_Tx_bit
IrqReqBDev3 * SerialTx_DevNo

IrqReqBPrio4 * serial_Rx_bit
IrqReqBDev4 * SerialRx_DevNo

IrqReqBPrio5 * podule_IRQ_bit
IrqReqBDev5 * Podule_DevNo

; Machine specific IRQB bits

 [ ReassignedIOMDInterrupts
IrqReqBPrio0 * IOMDr_MPEGAudio_IRQ_bit
IrqReqBDev0 * IOMDr_MPEGAudio_DevNo

IrqReqBPrio1 * IOMDr_MPEGVideo_IRQ_bit
IrqReqBDev1 * IOMDr_MPEGVideo_DevNo

IrqReqBPrio6 * IOMDr_Network_IRQ_bit
IrqReqBDev6 * IOMDr_Network_DevNo

IrqReqBPrio7 * IOMDr_serial_IRQ_bit
IrqReqBDev7 * IOMDr_Serial_DevNo
 |
IrqReqBPrio0 * IOMD_floppy_IRQ_bit
IrqReqBDev0 * DiscChanged_DevNo

IrqReqBPrio1 * IOMD_HardDisc_IRQ_bit
IrqReqBDev1 * Sound_DevNo

IrqReqBPrio6 * IOMD_Network_IRQ_bit
IrqReqBDev6 * WinnieIRQ_DevNo

IrqReqBPrio7 * IOMD_serial_IRQ_bit
IrqReqBDev7 * IOMD_Serial_DevNo
 ]

DTabC  SETA 1

IrqReqBDevnos
  =  NoInterrupt*3

  WHILE DTabC <256
  [ (DTabC:AND:IrqReqBPrio7)<>0
  = IrqReqBDev7*3
  |
  [ (DTabC:AND:IrqReqBPrio6)<>0
  = IrqReqBDev6*3
  |
  [ (DTabC:AND:IrqReqBPrio5)<>0
  = IrqReqBDev5*3
  |
  [ (DTabC:AND:IrqReqBPrio4)<>0
  = IrqReqBDev4*3
  |
  [ (DTabC:AND:IrqReqBPrio3)<>0
  = IrqReqBDev3*3
  |
  [ (DTabC:AND:IrqReqBPrio2)<>0
  = IrqReqBDev2*3
  |
  [ (DTabC:AND:IrqReqBPrio1)<>0
  = IrqReqBDev1*3
  |
  [ (DTabC:AND:IrqReqBPrio0)<>0
  = IrqReqBDev0*3
  ]
  ]
  ]
  ]
  ]
  ]
  ]
  ]
DTabC SETA DTabC+1
  WEND


 [ MorrisSupport
; Prioritised IRQD device numbers

IrqReqDPrio0     * 1:SHL:4
IrqReqDDev0      * IOMD_Event2_DevNo

IrqReqDPrio1     * 1:SHL:3
IrqReqDDev1      * IOMD_Event1_DevNo

IrqReqDPrio2     * 1:SHL:2
IrqReqDDev2      * IOMD_AtoD_DevNo

IrqReqDPrio3     * 1:SHL:1
IrqReqDDev3      * IOMD_MouseTxEmpty_DevNo

IrqReqDPrio4     * 1:SHL:0
IrqReqDDev4      * IOMD_MouseRxFull_DevNo


DTabC  SETA 1

IrqReqDDevnos
  =  NoInterrupt*3

; Top 3 bits are always 0 so table need only be 32 bytes (this will
; need to change if bits 5 to 7 are ever used).
  WHILE DTabC <32
  [ (DTabC:AND:IrqReqDPrio4)<>0
  = IrqReqDDev4*3
  |
  [ (DTabC:AND:IrqReqDPrio3)<>0
  = IrqReqDDev3*3
  |
  [ (DTabC:AND:IrqReqDPrio2)<>0
  = IrqReqDDev2*3
  |
  [ (DTabC:AND:IrqReqDPrio1)<>0
  = IrqReqDDev1*3
  |
  [ (DTabC:AND:IrqReqDPrio0)<>0
  = IrqReqDDev0*3
  ]
  ]
  ]
  ]
  ]
DTabC SETA DTabC+1
  WEND

; Prioritised IRQC device numbers. We have to handle ALL interrupts
; using specific device numbers as we don't know what the IO pins
; are connected to and the NOIRQ code has to know what bit to clear
; when an unknown interrupt is triggered.

IrqReqCPrio0    * 1:SHL:0
IrqReqCDev0     * IOMD_C_Bit0_DevNo

IrqReqCPrio1    * 1:SHL:1
IrqReqCDev1     * IOMD_C_Bit1_DevNo

IrqReqCPrio2    * 1:SHL:2
IrqReqCDev2     * IOMD_C_Bit2_DevNo

IrqReqCPrio3    * 1:SHL:3
IrqReqCDev3     * IOMD_C_Bit3_DevNo

IrqReqCPrio4    * 1:SHL:4
IrqReqCDev4     * IOMD_C_Bit4_DevNo

IrqReqCPrio5    * 1:SHL:5
IrqReqCDev5     * IOMD_C_Bit5_DevNo

IrqReqCPrio6    * 1:SHL:6
IrqReqCDev6     * IOMD_C_Bit6_DevNo

IrqReqCPrio7    * 1:SHL:7
IrqReqCDev7     * IOMD_C_Bit7_DevNo


DTabC   SETA 1

IrqReqCDevnos
  =  NoInterrupt*3

  WHILE DTabC <256
  [ (DTabC:AND:IrqReqCPrio7)<>0
  = IrqReqCDev7*3
  |
  [ (DTabC:AND:IrqReqCPrio6)<>0
  = IrqReqCDev6*3
  |
  [ (DTabC:AND:IrqReqCPrio5)<>0
  = IrqReqCDev5*3
  |
  [ (DTabC:AND:IrqReqCPrio4)<>0
  = IrqReqCDev4*3
  |
  [ (DTabC:AND:IrqReqCPrio3)<>0
  = IrqReqCDev3*3
  |
  [ (DTabC:AND:IrqReqCPrio2)<>0
  = IrqReqCDev2*3
  |
  [ (DTabC:AND:IrqReqCPrio1)<>0
  = IrqReqCDev1*3
  |
  [ (DTabC:AND:IrqReqCPrio0)<>0
  = IrqReqCDev0*3
  ]
  ]
  ]
  ]
  ]
  ]
  ]
  ]
DTabC SETA DTabC+1
  WEND
 ]

DefaultIRQ1Vcode_end

  ASSERT DefaultIRQ1Vcode_end - DefaultIRQ1Vcode <= DefIRQ1Vspace

 ] ; :LNOT: HAL


; +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

InitialiseIRQ1Vtable

        Push    "v1-v3,sb,lr"
; copy IRQ handler: not done with rest of copying
; because soft break needs the info to free any claimed blocks.

        LDR     a1, =DefaultIRQ1V
        ADRL    a2, DefaultIRQ1Vcode
        ADRL    a3, DefaultIRQ1Vcode_end
CopyDefaultIRQ1V
        LDR     a4, [a2], #4
        STR     a4, [a1], #4
        CMP     a2, a3
        BNE     CopyDefaultIRQ1V

  [ HAL
        AddressHAL
        ADD     v1, a1, #12
        ADD     a3, v1, #MaxInterrupts*12
        MOV     a2, #0
        MOV     a4, #-1
        LDR     ip, =IRQ
FillInDefaultIRQ1VDevices
        STMIA   a1!, {a2, a4, ip}
        ADD     a4, a4, #1
        CMP     a1, a3
        BNE     FillInDefaultIRQ1VDevices

; Now fill in our basic device handlers. First the timer.
        MOV     a1, #0
        CallHAL HAL_TimerDevice
        LDR     a2, =ZeroPage+OsbyteVars
        LDR     a3, =TickOne
        ADD     a1, a1, a1, LSL #1
        ADD     a1, v1, a1, LSL #2
        STMIB   a1, {a2, a3}

; Now the VSync
        MOV     a1, #0
        CallHAL HAL_VideoFlybackDevice
        CMP     a1, #-1
        LDRNE   a2, =ZeroPage+OsbyteVars
        LDRNE   a3, =VsyncIRQ
        ADDNE   a1, a1, a1, LSL #1
        ADDNE   a1, v1, a1, LSL #2
        STMNEIB a1, {a2, a3}
;        BNE     %ft1
;        ; here if no vsync handler .. fudge it from tickerv
;        MOV     a1, #TickerV
;        LDR     a2, =FalseVsyncIRQ
;        LDR     a3, =OsbyteVars
;        SWI     XOS_Claim

1
 [ CDVPoduleIRQs
; Now Podule bits
        MOV     a1, #IRQDesp_Link_Unshared
        LDR     a2, =ZeroPage+PFIQasIRQ_Chain - (PodDesp_Link-PodDesp_R12Val)
        ADR     a3, PFIQasIRQ_Despatch
        ADD     lr, v1, #8*12
        STMIA   lr, {a1, a2, a3}
        LDR     a2, =ZeroPage+PIRQ_Chain - (PodDesp_Link-PodDesp_R12Val)
        ADR     a3, PIRQ_Despatch
        ADD     lr, v1, #13*12
        STMIA   lr, {a1, a2, a3}
 ]

; Now IIC - if any
        LDR     v2, =ZeroPage+IICBus_Base
        MOV     v3, #0
80
        LDR     a1, [v2, #IICBus_Type]
        TST     a1, #IICFlag_HighLevel
        TSTNE   a1, #IICFlag_Background
        BEQ     %FT90
        SUB     sp, sp, #12
        MOV     a1, sp
        MOV     a2, v3
        CallHAL HAL_IICDevice
; I think it's safe to call A SWI here...
        LDMIA   sp!, {r0, r3, r4}
        LDR     r1, =IICIRQ
        MOV     r2, v3
        SWI     XOS_ClaimDeviceVector
90
        ADD     v2, v2, #IICBus_Size
        ADD     v3, v3, #1
        CMP     v3, #IICBus_Count
        BNE     %BT80
  ]
        Pull    "v1-v3,sb,pc"


; +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
; Specialist despatchers for podules

                  ^  0
PodDesp_Address   #  4     ; address of IRQ status byte
PodDesp_Mask      #  4     ; for use on above
PodDesp_R12Val    #  4
PodDesp_CallAddr  #  4     ; address to call if (?Address EOR (Mask>>8)) AND Mask <> 0
PodDesp_Link      #  4     ; next node
PodDesp_NodeSize  #  0

 [ HAL:LAND:{FALSE}
        ROUT
; In    r1 -> top level node
;       r12 = sub chain
01      LDR     r12, [r12, #PodDesp_Link-PodDesp_R12Val]
SubInterrupt_Despatch
        LDMIA   r12!, {r2, r3}          ; address and mask
        CMP     r3, #&10000
        LDRLOB  r2, [r2]
        BHS     %FT02
        EOR     r2, r2, r3, LSR #8      ; polarity inversion
        TST     r2, r3                  ; check against mask
        BEQ     %BT01
        LDMIA   r12, {r12, pc}
02
        Push    "r0,r1,r12,lr"
        MOV     r0, r3
        MOV     r12, r3
      [ NoARMv5
        MOV     lr, pc
        MOV     pc, r2
      |
        BLX     r2
      ]
        TEQ     r0, #0
        Pull    "r0,r1,r12,lr"
        BEQ     %BT01
        LDMIA   r12, {r12, pc}


NotSubInterrupt
        LDR     r1, [r1, #8]            ; call next (full) handler
        LDMIA   r1, {r12, pc}
 |

; In    r12 =    PFIQasIRQ_Chain - (PodDesp_Link-PodDesp_R12Val)
;             or PIRQ_Chain - (PodDesp_Link-PodDesp_R12Val)     from despatcher

PFIQasIRQ_Despatch ROUT
PIRQ_Despatch ; All the same thing now

01      LDR     r12, [r12, #PodDesp_Link-PodDesp_R12Val]
        LDMIA   r12!, {r1, r2}           ; address and mask

; TMD 09-Jun-89: Don't corrupt r0 - it's needed by the default IRQ2 routine
        LDRB    r1, [r1]
        EOR     r1, r1, r2, LSR #8
        ANDS    r1, r1, r2
        BEQ     %BT01
        LDMIA   r12, {r12, pc}
 ]

Default_SubInterruptHandler_Node
Default_PIRQHandler_Node
Default_PFIQasIRQHandler_Node
        &       .+4                     ; address we know has non-zero value!
        &       -1                      ; mask
        &       0                       ; handler r12
 [ HAL:LAND:{FALSE}
        &       NotSubInterrupt         ; handler code
 |
        &       IRQ                     ; handler code
 ]
        &       0                       ; null link for naff release checking

; +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
; Claim of device vectors

; r0 = Device number
; r1 = call address
; r2 = r12 value
 [ HAL
; r3 = interrupt location     or r3 = 0 or (if r4 > 64K)  r3 = routine
; r4 = interrupt mask/polarity                            r4 = workspace
 |
  [ CDVPoduleIRQs
; r0 = PFIQ|PIRQ devno -> r3 = interrupt location
;                         r4 = interrupt mask
  ]
 ]

CDV_Flags * &FF000000
CDV_Shared * 1:SHL:31

DeviceVector_Claim ROUT

        Push    "r0-r3, lr"

01      SWI     XOS_ReleaseDeviceVector ; Release until bored
        BVC     %BT01

        LDR     r0, [sp]
 [ HAL
        BIC     r0, r0, #CDV_Flags
 ]
        CMP     r0, #MaxInterrupts
        BHS     DV_Fail_NaffDevNo

 [ HAL:LAND:{FALSE}
;        TEQ     r3, #0
;        BNE     SubInterruptClaim
 |
  [ CDVPoduleIRQs
        CMP     r0, #Podule_DevNo
        CMPNE   r0, #PFIQasIRQ_DevNo
        BEQ     PoduleChainClaim
  ]
 ]

        MOV     r3, #12
        BL      ClaimSysHeapNode
        BVS     DV_Exit
        LDR     r11, [sp]
        BIC     r0, r11, #CDV_Flags
        ADD     r0, r0, r0, LSL #1      ; *3
        LDR     r1, =DefaultIRQ1V-DefaultIRQ1Vcode+Devices
        ADD     r1, r1, r0, LSL #2
        WritePSRc SVC_mode+I_bit, r10   ; IRQs off for update (on again on SWI exit)
        LDMIA   r1, {r0, r3, r10}
        STMIA   r2, {r0, r3, r10}       ; copy current head into node
 [ HAL
        MOV     r10, r2
        TST     r11, #CDV_Shared
        ORREQ   r10, r10, #IRQDesp_Link_Unshared
        LDR     r11, [sp, #4*2]         ; r12 value
        LDR     r12, [sp, #4*1]         ; call address
 |
        LDR     r10, [sp, #4*2]         ; r12 value
        LDR     r11, [sp, #4*1]         ; call address
        MOV     r12, r2
 ]
        STMIA   r1, {r10-r12}           ; copy given info into head


DV_Exit STRVS   r0, [sp]                ; Common exit for both claim + release
        Pull    "r0-r3, lr"
        B       SLVK_TestV


DV_Fail_NaffDevNo
        ADR     r0, ErrorBlock_NaffDevNo
      [ International
        BL      TranslateError
      |
        SETV
      ]
        B       DV_Exit

        MakeErrorBlock NaffDevNo

 [ HAL:LAND:{FALSE}
SubInterruptClaim
        Push    "r5"
        ADD     r0, r0, r0, LSL #1      ; *3
        LDR     r5, =DefaultIRQ1V-DefaultIRQ1Vcode+Devices
        ADD     r5, r5, r0, LSL #2
        LDR     r2, [r5, #IRQDesp_CallAddr]
        ADR     r3, SubInterrupt_Despatch
        TEQ     r2, r3
        BEQ     AlreadySubbed
        ; Need to claim the top-level interrupt.
        MOV     r3, #12
        BL      ClaimSysHeapNode
        Pull    "r5",VS
        BVS     DV_Exit
        WritePSRc SVC_mode+I_bit, r10   ; IRQs off for update (on again on SWI exit)
        LDMIA   r5, {r0, r3, r10}
        STMIA   r2, {r0, r3, r10}       ; copy current head into node
        ADR     r0, Default_SubInterruptHandler_Node
        ADR     r3, SubInterrupt_Despatch
        STR     r0, [r5, #IRQDesp_R12Val]
        STR     r3, [r5, #IRQDesp_CallAddr]
        STR     r2, [r5, #IRQDesp_Link]

AlreadySubbed
        ; r1 -> top level interrupt entry
        MOV     r3, #PodDesp_NodeSize
        BL      ClaimSysHeapNode
        Pull    "r5",VS
        BVS     DV_Exit
        MOV     r10, r2
        ADD     r1, sp, #8
        LDMFD   r1, {r1-r3}
        STR     r1, [r10, #PodDesp_CallAddr]
        STR     r2, [r10, #PodDesp_R12Val]
        STR     r3, [r10, #PodDesp_Address]
        STR     r4, [r10, #PodDesp_Mask]
        WritePSRc SVC_mode+I_bit, r2    ; IRQs off for update
        LDR     r0, [r5, #IRQDesp_R12Val]
        STR     r0, [r10, #PodDesp_Link]
        STR     r10, [r5, #IRQDesp_R12Val]
        Pull    "r5"
        B       DV_Exit
 ]
 [ CDVPoduleIRQs
PoduleChainClaim
        MOV     r3, #PodDesp_NodeSize
        BL      ClaimSysHeapNode
        BVS     DV_Exit
        MOV     r10, r2
        LDMFD   sp, {r0-r3}
        STR     r1, [r10, #PodDesp_CallAddr]
        STR     r2, [r10, #PodDesp_R12Val]
        STR     r3, [r10, #PodDesp_Address]
        STR     r4, [r10, #PodDesp_Mask]
        CMP     r0, #Podule_DevNo
        LDREQ   r0, =ZeroPage+PIRQ_Chain
        LDRNE   r0, =ZeroPage+PFIQasIRQ_Chain
        WritePSRc SVC_mode+I_bit, r1    ; IRQs off for update
        LDR     r1, [r0]
        STR     r1, [r10, #PodDesp_Link]
        STR     r10, [r0]
        B       DV_Exit
 ]

; .............................................................................
; Release of device vectors

; r0 = Device number
; r1 = call address
; r2 = r12 value
; r0 = PFIQ|PIRQ devno -> r3 = interrupt location (LDRB always used)
;                         r4 = interrupt mask

DeviceVector_Release ROUT

        Push    "r0-r3, lr"             ; Ensure same regset as above
 [ HAL
        BIC     r0, r0, #CDV_Flags
 ]
        CMP     r0, #MaxInterrupts
        BHS     DV_Fail_NaffDevNo

        WritePSRc SVC_mode + I_bit, r12 ; IRQs off while holding context
 [ CDVPoduleIRQs
        CMP     r0, #Podule_DevNo
        CMPNE   r0, #PFIQasIRQ_DevNo
        BEQ     PoduleChainRelease
 ]

        ADD     r0, r0, r0, LSL #1      ; *3
        LDR     r12, =DefaultIRQ1V-DefaultIRQ1Vcode+Devices
        ADD     r12, r12, r0, LSL #2    ; address of node
        MOV     r11, #-1                ; "fudge" predecessor node

 [ HAL
;        TEQ     r3, #0
;        BNE     SubInterruptRelease
 ]

 [ HAL
01      LDMIB   r12, {r3, r10}
 |
01      LDMIA   r12, {r3, r10}
 ]
        CMP     r3, r2
        CMPEQ   r10, r1
        BEQ     %FT02                   ; found it
        MOV     r11, r12
        LDR     r12, [r12, #IRQDesp_Link]
 [ HAL
        BICS    r12, r12, #IRQDesp_Link_Unshared
 |
        CMP     r12, #0
 ]
        BNE     %BT01

11      ADR     r0, ErrorBlock_BadDevVecRel
      [ International
        BL      TranslateError
      |
        SETV
      ]
        B       DV_Exit

        MakeErrorBlock BadDevVecRel


02      CMP     r11, #-1
        BEQ     %FT03
        MOV     r2, r12
        LDR     r12, [r2, #IRQDesp_Link]
 [ HAL
        LDR     r14, [r11, #IRQDesp_Link]       ; preserve r11's "unshared" flag
        BIC     r12, r12, #IRQDesp_Link_Unshared
        AND     r14, r14, #IRQDesp_Link_Unshared
        ORR     r12, r12, r14
 ]
        STR     r12, [r11, #IRQDesp_Link] ; node delinked
        B       %FT04

03      LDR     r2, [r12, #IRQDesp_Link]; freeable = nextnode
 [ HAL
        BIC     r2, r2, #IRQDesp_Link_Unshared
 ]
        LDMIA   r2,  {r0, r1, r3}       ; copy next node into head posn
        STMIA   r12, {r0, r1, r3}

04
        BL      FreeSysHeapNode         ; free block
        B       DV_Exit


 [ HAL:LAND:{FALSE}
SubInterruptRelease
        ADR     r10, SubInterrupt_Despatch
10      LDR     r0, [r12, #IRQDesp_CallAddr]
        TEQ     r0, r10
        BEQ     %FT15

13      MOV     r11, r12
        LDR     r12, [r12, #IRQDesp_Link]
        TEQ     r12, #0
        BEQ     %BT11
        B       %BT10

15      SUB     r0, r12, #PodDesp_Link
17      LDR     r14, [r0, #PodDesp_Link]
        TEQ     r14, #0
        BEQ     %BT13

        LDR     r10, [r14, #PodDesp_Address]
        CMP     r10, r3
        LDREQ   r10, [r14, #PodDesp_Mask]
        CMPEQ   r10, r4
        LDREQ   r10, [r14, #PodDesp_CallAddr]
        CMPEQ   r10, r1
        LDREQ   r10, [r14, #PodDesp_R12Val]
        CMPEQ   r10, r2
        MOVNE   r0, r14
        BNE     %BT17

        LDR     r10, [r14, #PodDesp_Link]
        STR     r10, [r0,  #PodDesp_Link]!

        LDR     r0, [r12, #IRQDesp_R12Val]
        MOV     r2, r14
        ADR     r14, Default_SubInterruptHandler_Node
        TEQ     r0, r14                 ; last sub-interrupt gone?
        BNE     %BT04

        BL      FreeSysHeapNode         ; free sub-interrupt
        BVS     DV_Exit
        B       %BT02                   ; then go back to delink top level
 ]
 [ CDVPoduleIRQs
PoduleChainRelease
        CMP     r0, #Podule_DevNo
        LDREQ   r0, =ZeroPage+PIRQ_Chain-PodDesp_Link
        LDRNE   r0, =ZeroPage+PFIQasIRQ_Chain-PodDesp_Link

10      LDR     r12, [r0, #PodDesp_Link]
        CMP     r12, #0
        BEQ     %BT11
        LDR     r11, [r12, #PodDesp_Address]
        CMP     r11, r3
        LDREQ   r11, [r12, #PodDesp_Mask]
        CMPEQ   r11, r4
        LDREQ   r11, [r12, #PodDesp_CallAddr]
        CMPEQ   r11, r1
        LDREQ   r11, [r12, #PodDesp_R12Val]
        CMPEQ   r11, r2
        MOVNE   r0, r12
        BNE     %BT10

        LDR     r11, [r12, #PodDesp_Link]
        STR     r11, [r0,  #PodDesp_Link]
        MOV     r2, r12
        B       %BT04
 ]

; +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
; Default device owner for IRQ not recognised by system: pass to IRQ2V

IRQ ROUT

        Push    "r10, lr"
        MOV     r10, #IrqV
        BL      CallVector

 [ HAL
        Pull    "r10, lr"
        Pull    "pc"                    ; new-style CDV - pull return address
 |
        Pull    "r10, pc"               ; return: someone will always claim it.
 ]

; *****************************************************************************
; Default IRQ2V:
 [ HAL
;   r0  must still have devno in it
;   r12 is 0 (from vector)
 |
;   r0  must still have devno*3 in it
;   r12 is 0 (from vector)
 ]

; Clear mask, clear IRQ as appropriate/possible

; NB. a cheap way of dividing by ~3 is *11,/32: accurate for 0..31 result ...

NOIRQ ROUT

 [ HAL
        TEQ     r0, #0
        Pull    pc, MI
        MOV     r11, r9
        AddressHAL
        CallHAL HAL_IRQDisable
        MOV     r9, r11
 |
01      SUBS    r0, r0, #3
        ADDGE   r12, r12, #1
        BGT     %BT01                   ; r12 := r0 DIV 3

        CMP     R12, #8
        MOVLO   R0, #IOCIRQMSKA
        BLO     %FT03

        CMP     R12, #16
        SUBLO   R12, R12, #8
        MOVLO   R0, #IOCIRQMSKB
        BLO     %FT03

        CMP     R12, #IOMD_MouseRxFull_DevNo
        SUBLO   R12, R12, #IOMD_DMAChannel0_DevNo
        MOVLO   R0, #IOMD_DMAMSK
        BLO     %FT03

 [ MorrisSupport
        CMP     R12, #IOMD_C_Bit0_DevNo
        SUBLO   R12, R12, #IOMD_MouseRxFull_DevNo       ;reduce to bit number 0..7
        MOVLO   R0, #IOMD_IRQMSKD                       ; in IRQ D interrupt register

        SUBHS   R12, R12, #IOMD_C_Bit0_DevNo
        MOVHS   R0, #IOMD_IRQMSKC
 ]

03
        ADD     r0, r0, #IOC
        MOV     r1, #1
        MOV     r1, r1, LSL r12         ; bit to clear

        MRS     lr, CPSR
        BIC     r12, lr, #&0F
        ORR     r12, r12, #I32_bit+F32_bit+IRQ_mode
        MSR     CPSR_c, r12
        LDRB    r12, [r0]       ; FIQs off for updating IOCIRQMSKA
        BIC     r12, r12, r1
        STRB    r12, [r0]       ; relevant IRQ disabled
        MSR     CPSR_c, lr      ; absolute minimum FIQ disable period

        STRB    r1, [r0, #IOCIRQCLRA-IOCIRQMSKA] ; Clear IRQ
 ]
        Pull    pc              ; claim vector

; +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
; The following bits have been appropriated from source.pmf.oseven to make
; sure Tim's old code doesn't overwrite us when he gets back!

; SWI OS_GenerateEvent: call event vector if enabled

GenEvent ROUT

        Push    lr
        WritePSRc SVC_mode+I_bit, lr    ; Disable IRQs. MUST call these ones
        BL      OSEVEN                  ; in SVC mode as people expect it
        Pull    lr
        B       SLVK

; +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
; Subroutine call version

; In    r0 = event type
;       r1,r2 parameters

; Out   C=0 => event was enabled, or was >= 32 anyway
;       C=1 => event was disabled, so vector not called

OSEVEN ROUT

        Push    lr

        CMP     r0, #31                 ; Events >= 32 are ALWAYS raised. SKS
                                        ; flags are HI if so, ie. NE
      [ ZeroPage = 0
        LDRLSB  r14, [r0, #OsbyteVars + :INDEX: EventSemaphores]
      |
        LDRLS   r14, =ZeroPage+OsbyteVars+:INDEX:EventSemaphores
        LDRLSB  r14, [r0, r14]
      ]
                                        ; get semaphore for this event 0..31
        CMPLS   r14, #0                 ; non-zero => enabled
        Pull    pc, EQ                  ; if disabled, exit with C=1

        Push    "r0-r3, r10-r12" ; r3 excessive ???
        MOV     r10, #EventV            ; call event vector
        BL      CallVector
        CLC                             ; indicate event enabled
        Pull    "r0-r3, r10-r12, pc"

; ...................... default owner of EventV ..............................
; Call Event handler

; In    r12 = EvtHan_ws

DefEvent ROUT

        MOV     lr, pc                  ; link with all the bits
        LDMIA   r12, {r12, pc}          ; call EventHandler, returns to ...

        TEQ     r12, #1
        Pull    pc,NE

      [ ZeroPage = 0
        LDRB    r14, [r12, #CallBack_Flag-1] ; IRQs are still disabled
        ORR     r14, r14, #CBack_OldStyle
        STRB    r14, [r12, #CallBack_Flag-1]
      |
        LDR     r12, =ZeroPage
        LDRB    r14, [r12, #CallBack_Flag] ; IRQs are still disabled
        ORR     r14, r14, #CBack_OldStyle
        STRB    r14, [r12, #CallBack_Flag]
      ]

        Pull    pc                      ; claim EventV

; +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
; Process timer zero IRQ device (100Hz clock)
; +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

        ALIGN 32

TickOne ROUT

 [ HAL
        ; Don't push r14 - we're using new interface, and claim the vector
        Push    "r9,r12"
        AddressHAL
        CallHAL HAL_IRQClear
        Pull    "r9,r12"
 |
        Push    r14

        MOV     R0, #timer0_bit
        STRB    R0, [R3, #IOCIRQCLRA]   ; clear timer 0 interrupt
 ]


        ! 0, "FIXME: temporary code"
;!!! HAPPY HACK
        STMFD   sp!,{r0-r12}
        ADR     r0,ret_from_vs
        Push    r0
        B       VsyncIRQ_ExtEntry
ret_from_vs
        LDMFD   sp!,{r0-r12}
;!!! HAPPY HACK

        LDR     R1, =ZeroPage
        LDR     R0, [R1, #MetroGnome]
        ADD     R0, R0, #1
        STR     R0, [R1, #MetroGnome]

        LDRB    R0, CentiCounter        ; Counter for VDU CTRL timing
        SUBS    R0, R0, #1
        STRCSB  R0, CentiCounter        ; decrement if not zero

        LDR     R0, IntervalTimer +0
        ADDS    R0, R0, #1              ; Increment the low 4 bytes
        STR     R0, IntervalTimer +0

        LDREQB  R0, IntervalTimer +4
        ADDEQ   R0, R0, #1              ; and carry into 5th byte if necessary
        STREQB  R0, IntervalTimer +4

        Push    "R4,R12"                ; R0-R3 already pushed

        TEQEQ   R0, #&100               ; has interval timer crossed zero ?
        MOVEQ   R0, #Event_IntervalTimer ; Event ITCZ
        BLEQ    OSEVEN

        BL      CentiSecondTick         ; Notify keyboard of a centisecond

        Pull    "R4,R12"

        LDR     R0, RealTime +0         ; Increment 5-byte real time
        ADDS    R0, R0, #1
        STR     R0, RealTime +0
        LDRCSB  R0, RealTime +4
        ADDCS   R0, R0, #1              ; Won't wrap until 2248 and then it
        STRCSB  R0, RealTime +4         ; all falls over anyway

        LDRB    R0, TimerState          ; get switch state
        TEQ     R0, #5                  ; toggles between 5 and 10

        LDREQ   R1, TimerAlpha +0       ; either load from one
        LDREQB  R2, TimerAlpha +4

        LDRNE   R1, TimerBeta +0        ; or the other
        LDRNEB  R2, TimerBeta +4

        ADREQ   R3, TimerBeta +0        ; and point to t'other
        ADRNE   R3, TimerAlpha +0

        ADDS    R1, R1, #1              ; increment
        ADC     R2, R2, #0              ; with carry

        STR     R1, [R3]                ; and store back
        STRB    R2, [R3, #4]

        EOR     R0, R0, #&0F            ; 5 <-> 10
        STRB    R0, TimerState

        Push    R10

 [ TickIrqReenter
        MOV     R10, #TickerV           ; call 100Hz vector
        BL      CallVector              ; IRQ's still disabled

        BL      ProcessTickEventChain   ; Re-enables IRQs
 |
        BL      ProcessTickEventChain

        MOV     R10, #TickerV           ; call 100Hz vector
        BL      CallVector
 ]
        Pull    "R10,PC"

; ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
; Process VSync IRQ device
; ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

        ALIGN 32
FalseVsyncIRQ ROUT
        LDR     R1, =ZeroPage
        LDR     R0, [R1, #MetroGnome]
        TST     R0, #1
        MOVEQ   pc, lr
        Push    "lr"
        B       VsyncIRQ_ExtEntry

VsyncIRQ ROUT

    [ HAL
        ; Don't push r14 - we're using new interface, and claim the vector
        Push    "r9,r12"
        AddressHAL
        CallHAL HAL_IRQClear
        Pull    "r9,r12"
    |
        Push    r14

        MOV     R0, #vsync_bit
        STRB    R0, [R3, #IOCIRQCLRA]   ; Clear the vsync interrupt
    ]

VsyncIRQ_ExtEntry
        LDRB    R0, CFStime             ; decrement 'CFS' timer !
        SUB     R0, R0, #1
        STRB    R0, CFStime

        VDWS    WsPtr                   ; Do our stuff before issuing VSYNC event
        BL      VsyncCall
        BYTEWS  WsPtr

        MOV     R0, #Event_VSync        ; VSYNC event number
        BL      OSEVEN

        LDRB    R1, FlashCount
        SUBS    R1, R1, #1
        Pull    PC, CC                  ; was zero, so frozen
        STRNEB  R1, FlashCount          ; else if now non-zero, store it back
        Pull    PC, NE                  ; not time to flash yet

        LDRB    R1, FlashState          ; Get the state and
        EORS    R1, R1, #1              ; flip to the other one (setting flags)
        STRB    R1, FlashState

        LDREQB  R2, SpacPeriod          ; get appropriate new period
        LDRNEB  R2, MarkPeriod
        STRB    R2, FlashCount          ; and put into counter

        VDWS    WsPtr
        Push    R4
        BEQ     dothesecondflash

dothefirstflash
        BL      DoFirstFlash
        Pull    "R4, PC"

dothesecondflash
        BL      DoSecondFlash
        Pull    "R4, PC"

; +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

        END