	
;******************************
;*
;* irqHcore
;*	

irqHcore

	push	r7
	movei	r4,2
	dexti	>UcHcoreIoBase
	movei	r3,UcHcoreIoBase
	ssto	InHcoreIoCtrl
	stwo	r3,r4		;request data
	nop	;wait for data become stable
	nop	;wait for data become stable
	
	rqldi	r3,InHcoreIoDataLow
	rqldi	r3,InHcoreIoDataHigh
	rqldi	r3,InHcoreIoCmdLow
	ld	r0			;data low
	ld	r1			;data high
	ld	r2			;cmd low
	rqldi	r3,InHcoreIoCmdHigh
	movei	r5,$f			;only 16 hcores supported
	nop
	ld	r4			;cmd high
	and	r5,r2,r5		;r5 = cmd id
	
	;nop	;debug
	;dump	;debug
	
	;jump to cmd
	
	dexti	>hcoreFuncTable
	movei	r3,hcoreFuncTable
	push	r4			;store cmd high
	rqld	r3,r5
	nop
	nop
	ld	r3	;ld function address
	gpci	r7,2	
	jmpi	r3,0
	nop		;delay slot
	nop		;delay slot
	nop		;delay slot
	nop		;delay slot
	
	;result processing if needed (r5:r6 = result)
	
	rqpop
	dexti	>UcHcoreIoBase
	movei	r3,UcHcoreIoBase
	pop	r4	;restore cmd high
	movei	r0,1	;used for write out later
	extri	r4,15	;bit 15 true is return value
	brtc	irqHcoreNoReturn
	ssto	InHcoreIoDataLow	;store return value (also if no return) delay slot
	stwo	r3,r6			;delay slot
	ssto	InHcoreIoDataHigh	;delay slot
	stwo	r3,r5			;delay slot
	
	;store result
	
	ssto	InHcoreIoCmdHigh
	stwo	r3,r4		
	ssto	InHcoreIoCtrl
	stwo	r3,r0	;write out
	
irqHcoreNoReturn	
	
	;clear irq and restore lr
	
	rqpop
	dexti	>UcIrqMemory
	movei	r0,UcIrqMemory
	pop	r7
	rqldi	r0,0
	nop
	movei	r2,$10		;hcore
	ld	r1		;memory
	jmpi	r7,0
	bic	r1,r1,r2	;			delay slot
	nop			;			delay slot	
	st	r0,r1		;store irq memory	delay slot
	nop			;			delay slot
	
	
hcoreFuncTable

	word	hcnop		;0 ok
	word	hcsetparam      ;1 ok
	word	hcgetparam      ;2 ok
	word	hccall          ;3 ok
	word	hcsemst         ;4
	word	hcsemld         ;5
	word	hcseminc        ;6
	word	hcsemdec        ;7
	word	hcallocsem      ;8
	word	hcfreesem       ;9
	word	hcnop		;10
	word	hcnop		;11
	word	hcnop		;12
	word	hcnop		;13
	word	hcnop		;14
	word	hcnop		;15
	
	
;******************************
;*
;* hcnop
;* 	
;* r1:r0	data
;* r4:r2	cmd
;*
;* r5:r6	return value
;*	

        
hcsemst      
hcsemld      
hcseminc     
hcsemdec     
hcallocsem   
hcfreesem    	
hcnop	jmpi	r7,0	
	or	r5,r4,r4	;return cmd 	delay slot
	or	r6,r2,r2	;		delay slot
	nop			;		delay slot		
	nop			;		delay slot
	
	
;******************************
;*
;* hcsetparam
;* 	
;* r1:r0	data
;* r4:r2	cmd
;*
;* r5:r6	return value
;*		
	
hcsetparam
	
	dexti	>paramMemory
	movei	r3,paramMemory
	lsri	r4,7	;
	swp	r2,r2
	moveih	r4,0		;hcore id
	moveih	r2,0		;param pos
	muli	r4,32		;32 words offset
	add	r2,r2,r2	;param pos * 2
	add	r3,r3,r4	;pm += id * 32
	movei	r5,0		;return 0
	add	r3,r3,r2	;pm += pos * 2
	jmpi	r7,0		;
	movei	r6,0		;return 0	delay slot
	st	r3,r1		;store high	delay slot
	ssto	1		;		delay slot
	stwo	r3,r0		;store low	delay slot
	
;******************************
;*
;* hcgetparam
;* 	
;* r1:r0	data
;* r4:r2	cmd
;*
;* r5:r6	return value
;*
	
hcgetparam

	dexti	>paramMemory
	movei	r3,paramMemory
	lsri	r4,7	;
	swp	r2,r2
	moveih	r4,0		;hcore id
	moveih	r2,0		;param pos
	muli	r4,32		;32 words offset
	add	r2,r2,r2	;param pos * 2
	add	r3,r3,r4	;pm += id * 32
	movei	r5,0		;return 0
	add	r3,r3,r2	;pm += pos * 2
	nop
	rqldi	r3,1
	rqldi	r3,0
	jmpi	r7,0	
	ld	r6	;low param	delay slot	
	ld	r5	;high param	delay slot
	nop		;		delay slot
	nop		;		delay slot
	
;******************************
;*
;* hccall
;* 	
;* r1:r0	data
;* r4:r2	cmd
;*
;* r5:r6	return value
;*
			
hccall 	push	r7
	movei	r3,$f
	dexti	>hccallFuncTable
	movei	r5,hccallFuncTable
	and	r0,r0,r3	;max 16 functions
	lsri	r4,7	;	;core id
	rqld	r5,r0		;request function address
	moveih	r4,0
	nop
	ld	r5
	
	;nop	;debug
	;dump	;debug
	
	;call function
	
	gpci	r7,2
	jmpi	r5,0
	nop			;		delay slot
	nop			;		delay slot
	nop			;		delay slot		
	nop			;		delay slot
	
	rqpop
	nop
	nop
	pop	r7
	nop

	jmpi	r7,0
	nop			;		delay slot
	nop			;		delay slot
	nop			;		delay slot		
	nop			;		delay slot
	

hccallFuncTable

	word	hccallNop        ;0
	word	hccallSetLed     ;1
	word	hccallUCtrlWrite ;2 
	word	hccallUCtrlRead  ;3 
	word	hccallNop        ;4
	word	hccallNop        ;5
	word	hccallNop        ;6
	word	hccallNop        ;7
	word	hccallNop        ;8
	word	hccallNop        ;9
	word	hccallNop        ;10
	word	hccallNop        ;11
	word	hccallNop        ;12
	word	hccallNop        ;13
	word	hccallNop        ;14
	word	hccallNop        ;15

;******************************
;*
;* hccallNop
;*
;* r4 = id 
;*	
;* r5:r6	return value
;*	

	
hccallNop

	jmpi	r7,0
	nop			;		delay slot
	nop			;		delay slot
	nop			;		delay slot		
	nop			;		delay slot
	
;************************************************************
;*
;* hccallSetLed (use param 0 to set/clr led[id])
;*
;* r4 = id 
;*	
;* r5:r6	return value
;*	
	
hccallSetLed

	or	r5,r4,r4	;store id
	dexti	>paramMemory
	movei	r3,paramMemory
	muli	r5,32		;goto paramMemory[id]
	addi	r3,1		;low param
	dexti	>ledBuffer
	movei	r6,ledBuffer
	rqld	r3,r5		;get param 0
	rqldi	r6,1		;get ledbuffer content
	movei	r1,0
	ld	r0		;param 0
	ld	r5		;led buffer value
		
	bset	r1,r4		;1 << id
	cmpeqi	r0,0		;led off
	bicts	r5,r1		;clear bit out
	tnt
	orts	r5,r1		;else set
	
	dexti	>UcRSwichtWLedG		
	movei	r1,UcRSwichtWLedG       
	jmpi	r7,0
	st	r6,r5           ;store to ledbuffer	delay slot
	st	r1,r5		;write rleds		delay slot
	movei	r5,0		;return 0		delay slot
	movei	r6,0            ;return 0		delay slot
	
ledBuffer	word	0
	
;************************************************************
;*
;* hccallUCtrlWrite (address = param0,data = param 1)
;*
;* r4 = id 
;*	
;* r5:r6	return value
;*	

hccallUCtrlWrite

	dexti	>paramMemory
	movei	r3,paramMemory
	muli	r4,32		;goto paramMemory[id]
	nop
	add	r3,r3,r4	;paramMemory[id]
	movei	r4,UcBase
	rqldi	r3,1		;param0 [low]
	rqldi	r3,3		;param1 [low]
	moveih	r4,>UcBase
	ld	r0		;adr
	ld	r1		;data
			
	cmplo	r0,r4		;only write over base $8000
	jmpts	r7		;go back with error
	movei	r5,$ff		;			delay slot
	movei	r6,$ff          ;			delay slot
	moveih	r5,$ff		;			delay slot
	moveih	r6,$ff          ;return $ffffffff	delay slot
	
	;else
	
	jmpi	r7,0
	st	r0,r1		;uctrl[r0] = r1		delay slot
	movei	r5,0		;			delay slot
	movei	r6,0            ;return 0		delay slot
	nop			;			delay slot
	
;************************************************************
;*
;* hccallUCtrlRead (address = param0)
;*
;* r4 = id 
;*	
;* r5:r6	return value
;*	

hccallUCtrlRead

	dexti	>paramMemory
	movei	r3,paramMemory
	muli	r4,32		;goto paramMemory[id]
	nop
	add	r3,r3,r4	;paramMemory[id]
	movei	r4,UcBase
	rqldi	r3,1		;param0 [low]
	nop
	moveih	r4,>UcBase
	ld	r0		;adr
	nop
	cmplo	r0,r4		;only read over base $8000
	jmpts	r7		;go back with error
	movei	r5,$ff		;			delay slot
	movei	r6,$ff          ;			delay slot
	moveih	r5,$ff		;			delay slot
	moveih	r6,$ff          ;return $ffffffff	delay slot
	
	;else

	jmpi	r7,0		;
	rqldi	r0,0		;			delay slot
	nop			;			delay slot
	movei	r5,0            ;			delay slot
	ld	r6		;return $0000:VALUE	delay slot
	
	;

;*********************************************************************
;* data
;*********************************************************************	
	
paramMemory	;16 longs per core

	.space	512	;32*16
		