; 26bit vector handlers

#set max_vectors_to_track = 32			;max number of vectors we allow



STRUCT vector_table
{
  .entry_code		DCD 0	;if >0 its our managed entry point
  .original_code	DCD 0	;original entry point
  .type			DCD 0	;SWI # used to create
  .parameter		DCD 0
  .private		DCD 0
  .counter		DCD 0	;track number of claims
  .taskID		DCD 0	;task associated with this claim
  .size
}




;initialise_vector_table
;-----------------------
;called by GOARM3JIT to initialise the vector handlers in JIT appspace
;
;
;Entry:
; R0 - JIT codlet space (FD stacked codelets)

.initialise_vector_table
 STMFD   R13!, {R1-R7, R14}
 TEMP    R7
 SUB     R0, R0, #max_vectors_to_track * vector_table.size
 MOV     R2, #0
 MOV     R1, #max_vectors_to_track * vector_table.size
 ._vectors_L1
   STR     R2, [R0, #-4]!
   SUBS    R1, R1, #4
 BNE     _vectors_L1
 STR     R0, jit_claimed_vectors

 MOV     R6, #max_vectors_to_track
 ADR     R1, _Vector_Claim_Handlers
 LDMIA   R1, {R1, R2, R4}
 ADR     R5, call_vector_claim
 ._vectors_L2
   SUBS     R6, R6, #1
   ORR      R3, R2, R6
   STMDB    R0!, {R1, R3, R4, R5}
 BNE     _vectors_L2
 STR     R0, Vector_Claim_Handlers
 LOCK    R7
LDMFD   R13!, {R1-R7, PC}

 ._Vector_Claim_Handlers
 STMFD   R13!, {R4-R7, R14}
 MOV     R6, #0			;R6=vector table #
 LDR     PC, [PC, #-4]
 ; DCD 0	;address of call_vector_claim







;kill_vectors_in_range
;---------------------
;removes any vector claims within an address range

;Entry:
; R8 - start address
; R9 - end address

.kill_vectors_in_range
 STMFD   R13!, {R0-R2, R6-R7, R10-R12, R14}

 BL      return_taskID
 MOV     R11, R10			;R11=taskID
 LDR     R7, jit_claimed_vectors
 BL      _check_vectors

 MOV     R1, #0
 ADR     R12, DeviceVectors
 LDR     R0, [R12, #(IOC_VSync_Pulse * device_table.size) + device_table.taskID]
 TEQ     R0, R11				;does the taskID match?
 BNE     _P3					;NO
 LDR     R0, [R12, #(IOC_VSync_Pulse * device_table.size) + device_table.original_code]
 CMP     R0, R8					;within address range?
 CMPHS   R9, R0					;YES, disable
 STRHI   R1, [R12, #(IOC_VSync_Pulse * device_table.size) + device_table.entry_code]

 ._P3
 LDR     R0, [R12, #(IOC_Timer_0 * device_table.size) + device_table.taskID]
 TEQ     R0, R11				;does the taskID match?
 BNE     _P4					;NO
 LDR     R0, [R12, #(IOC_Timer_0 * device_table.size) + device_table.original_code]
 CMP     R0, R8					;within address range?
 CMPHS   R9, R0					;YES, disable
 STRHI   R1, [R12, #(IOC_Timer_0 * device_table.size) + device_table.entry_code]

 ._P4
 LDR     R0, [R12, #(IOC_Timer_1 * device_table.size) + device_table.taskID]
 TEQ     R0, R11				;does the taskID match?
 BNE     _P5					;NO
 LDR     R0, [R12, #(IOC_Timer_1 * device_table.size) + device_table.original_code]
 CMP     R0, R8					;within address range?
 CMPHS   R9, R0					;YES, disable
 STRHI   R1, [R12, #(IOC_Timer_1 * device_table.size) + device_table.entry_code]

 ._P5
 LDR     R0, [R12, #(IOC_SSBC * device_table.size) + device_table.taskID]
 TEQ     R0, R11				;does the taskID match?
 BNE     _exit					;NO
 LDR     R0, [R12, #(IOC_SSBC * device_table.size) + device_table.original_code]
 CMP     R0, R8					;within address range?
 CMPHS   R9, R0					;YES, disable
 STRHI   R1, [R12, #(IOC_SSBC * device_table.size) + device_table.entry_code]

 ._exit
LDMFD   R13!, {R0-R2, R6-R7, R10-R12, PC}



._check_vectors
 STMFD   R13!, {R14}
 MOV     R12, #max_vectors_to_track	;release vectors in reverse order
 ._L1
   LDR     R0, [R7, #vector_table.entry_code]	;address of codelet
   TEQ     R0, #0			;already released?
   BEQ     _P1				;YES

   LDR     R14, [R7, #vector_table.taskID]
   TEQ     R14, R11			;does the taskID match?
   BNE     _P1				;NO

   LDR     R1, [R7, #vector_table.original_code]
   CMP     R1, R8			;within address range?
   CMPHS   R9, R1
   BLO     _P1				;NO

   LDR     R14, [R7, #vector_table.type]	;get the SWI used

   TEQ     R14, #&1F << 12		;OS_Claim		PRM1-66
   TEQNE   R14, #&47 << 12		;OS_AddToVector		PRM1-72
   MOVEQ   R10, #&20	 		;OS_Release		PRM1-68
   BEQ     _parameter

   TEQ     R14, #&3B << 12		;OS_CallAfter		PRM1-441
   TEQNE   R14, #&3C << 12		;OS_CallEvery		PRM1-443
   MOVEQ   R10, #&3D	 		;OS_RemoveTickerEvent	PRM1-445

   TEQ     R14, #&54 << 12		;OS_AddCallBack		PRM1-324
   MOVEQ   R10, #&5F			;OS_RemoveCallBack	PRM1-327

   LDR     R6, [R7, #vector_table.counter]	;get the claim counter
   ._no_parameter
   LDR     R1, [R7, #vector_table.private]	;R12 value
   B       _release_vector

   ._parameter
   MOV     R1, R0				;address of codelet
   LDR     R0, [R7, #vector_table.parameter]	;original parameter
   LDR     R2, [R7, #vector_table.private]	;R12 value

   ._release_vector
   ORR     R10, R10, #swi_X_bit			;X form of SWI
   ._L2
     SWI     XOS_CallASWI
     BVS     _P2				;exit early on error
     SUBS    R6, R6, #1
   BGT     _L2

   ._P2
   MOV     R0, #0
   STR     R0, [R7, #vector_table.entry_code]	;mark as released
   STR     R0, [R7, #vector_table.taskID]
   STR     R0, [R7, #vector_table.counter]

   ._P1
   ADD     R7, R7, #vector_table.size
   SUBS    R12, R12, #1
 BNE     _L1
LDMFD   R13!, {PC}




; Entry: in either IRQ or SVC, IRQ may or may not be enabled
; STACK: R4-R7, R14 (32bit return)
; R6    - Vector table #

.call_vector_claim
; Entry:
 LDR     R14, jit_claimed_vectors
 MOV     R7, #vector_table.size
 MRS     R5, CPSR			;R5=entry CPU mode
 MLA     R6, R7, R6, R14		;R0=vector info
 LDR     R14, [R6, #vector_table.type]	;need to clear CallAfter's
 LDR     R7, [R6, #vector_table.original_code]
 TEQ     R14, #&1F << 12		;was claim via OS_Claim?
 BEQ     _os_claim			;YES
 TEQ     R14, #&3B << 12		;was claim via OS_CallAfter?
 TEQNE   R14, #&54 << 12		;was claim via OS_AddCallBack?
 BNE     _not_callafter			;NO
 LDR     R14, [R6, #vector_table.counter]	;decrease the claim counter
 SUBS    R14, R14, #1
 STRPL   R14, [R6, #vector_table.counter]

 STREQ   R14, [R6, #vector_table.entry_code]	;allow the slot to be used
 STREQ   R14, [R6, #vector_table.type]
 STREQ   R14, [R6, #vector_table.original_code]
 STREQ   R14, [R6, #vector_table.taskID]
 B       _not_callafter

 ._entry_mode		DCD 0
 ._original_code	DCD 0

 ._not_callafter
 MRS     R6, CPSR
 ORR     R6, R6, #%10 << 6
 MSR     CPSR_c, R6			;IRQ off

 ADD     R7, R7, #jit_code_start
 STR     R5, _entry_mode
 STR     R7, _original_code
 LDMFD   R13!, {R4-R7}			;clear down stack
 #if compile_RO > 400
   MSR     CPSR_c, #%10010011		;SVC32, IRQ disabled
 #endif
 #if compile_RO == 400
   MSR     CPSR_c, #%10000011		;SVC26, IRQ disabled
 #endif
 #if compile_IOMD == 1
   NOP
 #endif

 STR     R14, [R13, #-4]!		;preserve SVC R14
 MRS     R14, SPSR
 STR     R14, [R13, #-4]!		;preserve SVC SPSR
 LDR     R14, _entry_mode
 SUB     R13, R13, #4 * 4		;space for stack#, USR R13, R14
; SVC stack: <spare>, <spare>, <spare>, <spare>, SVC SPSR, SVC R14

 STR     R14, [R13, #3*4]		;preserve Entry mode and
					;..fix for errata on SA-110 rev.2
 STMIB   R13, {R13-R14}^		;preserve USR R13, R14
; SVC stack: <spare>, USR R13, USR R14, <entry mode>, SVC SPSR, SVC R14
 NOP
 STMFD   R13!, {R0-R1}
 BL      Allocate_Transient_Stack	;allocate a stack
 STR     R0, [R13, #-4]!		;stack address
 LDMIA   R13, {R13}^			;set USR stack
 #if compile_IOMD == 1
   NOP
 #endif
 ADD     R13, R13, #4
 STR     R1, [R13, #2 * 4]		;transient_stack_no
; SVC stack: R0, R1, <stack no>, USR R13, USR R14, <entry mode>, SVC SPSR, SVC R14

 LDR     R0, _entry_mode
 TST     R0, #%10000000			;IRQ enabled?
 LDMFD   R13!, {R0-R1}

 BEQ     _irq_enabled
 #if compile_RO > 400
   MSR     CPSR_c, #%10010000		;USR32 IRQ disabled
 #endif
 #if compile_RO == 400
   MSR     CPSR_c, #%10000000		;USR26 IRQ disabled
 #endif
 #if compile_IOMD == 1
   NOP
 #endif
 B       _now_in_USR

 ._irq_enabled
 #if compile_RO > 400
   MSR     CPSR_c, #%00010000		;USR32 IRQ enabled
 #endif
 #if compile_RO == 400
   MSR     CPSR_c, #%00000000		;USR26 IRQ enabled
 #endif
 #if compile_IOMD == 1
   NOP
 #endif
 ._now_in_USR

 ADR     R14, _return_claimed
 STR     R14, [R13, #-4]!
 ADR     R14, _return
 STR     R14, [R13, #-4]!

 LDR     R14, _original_code
 STR     R14, [R13, #-8]!		;space for return
 LDR     R14, jit_managed_ldr_pc_r13_2	;hypervised LDR PC, [R13+4], #4
 ORR     R14, R14, #%10 << 26		;return in USR with IRQ disabled
 STR     R14, [R13, #4]			;stack IRQ return address
 LDR     PC, [R13], #4			;call claimant

 ._r0	DCD 0

; SVC stack: <stack no>, USR R13, USR R14, <entry mode>, SVC SPSR, SVC R14

 ._return
 STMFD   R13!, {R0, R4-R5}
 MOV     R4, R13			;R4={R0, R4-R5}
 MRS     R5, CPSR			;R5=exit CPSR
 SWI     XOS_EnterOS			;PRM1-141
 #if compile_RO > 400
   MSR     CPSR_c, #%10010011		;SVC32 IRQ disabled
 #endif
 #if compile_RO == 400
   MSR     CPSR_c, #%10000011		;SVC26 IRQ disabled
 #endif
 #if compile_IOMD == 1
   NOP
 #endif

 LDR     R0, [R13, #3 * 4]		;get entry mode from stack
 AND     R5, R5, #%1111 << 28
 BIC     R0, R0, #%1111 << 28
 ORR     R0, R0, R5			;copy exit flags
 STR     R0, [R13, #3 * 4]
 LDMIA   R4, {R0, R4-R5}		;restore R0, R4-R5
 STR     R0, _r0
 LDR     R0, [R13, #0]			;stack no
 BL      Free_Transient_Stack

 LDMIB   R13, {R13, R14}^		;preserve USR R13, R14
 NOP
 ADD     R13, R13, #3 * 4
; SVC stack: <entry mode>, SVC SPSR, SVC R14

 LDR     R14, [R13, #4]			;preserve SVC SPSR
 MSR     SPSR_all, R14
 LDR     R14, [R13, #8]			;preserve SVC R14
 LDR     R0, [R13], #12
 MSR     CPSR_all, R0			;back into entry CPU mode
 #if compile_IOMD == 1
   NOP
 #endif
 LDR     R0, _r0
LDR     PC, [R13], #4


; SVC stack: <stack no>, USR R13, USR R14, <entry mode>, SVC SPSR, SVC R14

 ._return_claimed
 STMFD   R13!, {R0, R4-R5}
 MOV     R4, R13			;R4={R0, R4-R5}
 MRS     R5, CPSR			;R5=exit CPSR
 SWI     XOS_EnterOS			;PRM1-141
 #if compile_RO > 400
   MSR     CPSR_c, #%10010011		;SVC32 IRQ disabled
 #endif
 #if compile_RO == 400
   MSR     CPSR_c, #%10000011		;SVC26 IRQ disabled
 #endif
 #if compile_IOMD == 1
   NOP
 #endif

 LDR     R0, [R13, #3 * 4]		;get entry mode from stack
 AND     R5, R5, #%1111 << 28
 BIC     R0, R0, #%1111 << 28
 ORR     R0, R0, R5			;copy exit flags
 STR     R0, [R13, #3 * 4]
 LDMIA   R4, {R0, R4-R5}		;restore R0, R4-R5
 STR     R0, _r0
 LDR     R0, [R13, #0]			;stack no
 BL      Free_Transient_Stack

 LDMIB   R13, {R13, R14}^		;preserve USR R13, R14
 #if compile_IOMD == 1
   NOP
 #endif
 ADD     R13, R13, #3 * 4
; SVC stack: <entry mode>, SVC SPSR, SVC R14

 LDR     R14, [R13, #4]			;preserve SVC SPSR
 MSR     SPSR_all, R14
 LDR     R14, [R13, #8]			;preserve SVC R14
 LDR     R0, [R13], #12
 MSR     CPSR_all, R0			;back into entry CPU mode
 #if compile_IOMD == 1
   NOP
 #endif
 LDR     R0, _r0
LDMFD   R13!, {R14, PC}


; Entry:
; Stack: R4-R7, R14 (32bit return)
; R6    - Vector table #
 ._os_claim				;filter out RO5 UpCall's
 LDR     R14, [R6, #vector_table.parameter]
 TEQ     R14, #29			;UpCallV?
 BNE     _not_callafter

; *** workaround for WiFi stack triggering SDIODriver spinlocks on Pi's ***
 TEQ     R0, #6				;UpCall 6?
#if JIT_debug == 1
  TEMP R4
  STREQ R0, jit_write_aborts
  LOCK R4
; STREQ   R0, [R1, #0]
; MOVEQ   R0, #0				;YES, claim it
#endif
 LDMEQFD R13!, {R4-R7, PC}		; |+  and exit
; *************************************************************************

 CMP     R0, #12			;is it a legacy UpCall?
 #if High_Vectors == 0
   BLS     _not_callafter
 #endif
 #if High_Vectors == 1
   BHI     _not_upcall			;NO
   CMP     R0, #2			;YES, is it UpCall 1 or 2?
   ADR     R6, VARS
   STRLS   R2, [R6, #page_zero_UpCall - VARS]	;YES
   B       _not_callafter		;YES
   ._not_upcall
 #endif
 EORS    R14, R0, #256
 EORNES  R14, R14, #1			;257
 BEQ     _not_callafter			;YES
LDMFD   R13!, {R4-R7, PC}		;NO
