path: root/big-little/lib/virt_helpers.s

	;
	; Copyright (c) 2012, ARM Limited. All rights reserved.
	;
	; Redistribution and use in source and binary forms, with
	; or without modification, are permitted provided that the
	; following conditions are met:
	;
	; Redistributions of source code must retain the above
	; copyright notice, this list of conditions and the
	; following disclaimer.
	;
	; Redistributions in binary form must reproduce the
	; above copyright notice, this list of conditions and
	; the following disclaimer in the documentation
	; and/or other materials provided with the distribution.
	;
	; Neither the name of ARM nor the names of its
	; contributors may be used to endorse or promote products
	; derived from this software without specific prior written
	; permission.
	;


	IMPORT read_actlr
	IMPORT write_actlr

	EXPORT smc
	EXPORT dcisw
	EXPORT dccsw
	EXPORT dccisw
	EXPORT read_lr
	EXPORT read_sp
	EXPORT write_sp
	EXPORT write_lr
	EXPORT panic
	EXPORT spin_lock
	EXPORT spin_trylock
	EXPORT spin_unlock
	EXPORT virt_memset
	EXPORT hyp_save
	EXPORT num_secondaries
	EXPORT virt_dead
	EXPORT get_sp
	EXPORT disable_coherency
	EXPORT enable_coherency
	EXPORT inv_tlb_all
	EXPORT inv_tlb_mva
	EXPORT inv_icache_all
	EXPORT inv_bpred_is
	EXPORT inv_bpred_all
	EXPORT inv_icache_mva_pou
	EXPORT inv_dcache_mva_poc
	EXPORT cln_dcache_mva_pou
	EXPORT cln_dcache_mva_poc
	EXPORT cache_maint_op

; Cache maintenance op types
INV	EQU	0x0
CLN	EQU	0x1
CLN_INV	EQU	0x2

	AREA |.text|, CODE

read_lr FUNCTION
	; Save r1
	push	{r1}
	and	r0, r0, #0x1f
	; Read the current cpsr
	mrs	r1, cpsr
	and	r1, r1, #0x1f
	; Check if the desired lr is of the current mode
	cmp	r0, r1
	moveq   r0, LR
	beq	read_lr_out
	; Check if desired lr is of user mode
	cmp	r0, #0x10
	mrseq   r0, LR_usr
	beq	read_lr_out
	; Check if desired lr is of supervisor mode
	cmp	r0, #0x13
	mrseq   r0, LR_svc
read_lr_out
	pop	{r1}
	bx      lr
	ENDFUNC

write_lr FUNCTION
	; Save r2
	push	{r2}
	and	r0, r0, #0x1f
	; Read the current cpsr
	mrs	r2, cpsr
	and	r2, r2, #0x1f
	; Check if the lr is of the current mode
	cmp	r0, r2
	moveq   LR, r1
	beq	write_lr_out
	; Check if the lr is of user mode
	cmp	r0, #0x10
	msreq   LR_usr, r1
	beq	write_lr_out
	; Check if the lr is of supervisor mode
	cmp	r0, #0x13
	msreq   LR_svc, r1
write_lr_out
	pop	{r2}
	bx      lr
	ENDFUNC

read_sp FUNCTION
	; Save r1
	push	{r1}
	and	r0, r0, #0x1f
	; Read the current cpsr
	mrs	r1, cpsr
	and	r1, r1, #0x1f
	; Check if the desired sp is of the current mode
	cmp	r0, r1
	moveq   r0, SP
	beq	read_sp_out
	; Check if desired sp is of user mode
	cmp	r0, #0x10
	mrseq   r0, SP_usr
	beq	read_sp_out
	; Check if desired sp is of supervisor mode
	cmp	r0, #0x13
	mrseq   r0, SP_svc
	beq	read_sp_out
	; Check if desired sp is of irq mode
	cmp	r0, #0x12
	mrseq   r0, SP_irq
	beq	read_sp_out
	; Check if desired sp is of supervisor mode
	cmp	r0, #0x1a
	mrseq   r0, SP_hyp
	beq	read_sp_out
	; Check if desired sp is of monitor mode
	cmp	r0, #0x16
	mrseq   r0, SP_mon
read_sp_out
	pop	{r1}
	bx      lr
	ENDFUNC

write_sp FUNCTION
	; Save r2
	push	{r2}
	and	r0, r0, #0x1f
	; Read the current cpsr
	mrs	r2, cpsr
	and	r2, r2, #0x1f
	; Check if the sp is of the current mode
	cmp	r0, r2
	moveq   SP, r1
	beq	write_sp_out
	; Check if the sp is of user mode
	cmp	r0, #0x10
	msreq   SP_usr, r1
	beq	write_sp_out
	; Check if the sp is of supervisor mode
	cmp	r0, #0x13
	msreq   SP_svc, r1
	beq	write_sp_out
	; Check if the sp is of irq mode
	cmp	r0, #0x12
	msreq   SP_irq, r1
	beq	write_sp_out
	; Check if the sp is of hyp mode
	cmp	r0, #0x1a
	msreq   SP_hyp, r1
	beq	write_sp_out
	; Check if the sp is of monitor mode
	cmp	r0, #0x16
	msreq   SP_mon, r1
write_sp_out
	pop	{r2}
	bx      lr
	ENDFUNC

	ALIGN   4

;--------------------------------------------------------
; spin_lock
;--------------------------------------------------------
spin_lock FUNCTION
	MOV     r2, #1
sl_tryloop
	LDREX   r1, [r0]
	CMP	r1, #0
	STREXEQ r1, r2, [r0]
	CMPEQ   r1, #0
	BNE     sl_tryloop
	MCR     p15, 0, r0, c7, c10, 4
	bx      lr
	ENDFUNC

;--------------------------------------------------------
; spin_lock
;--------------------------------------------------------
spin_trylock FUNCTION
	MOV     r2, #1
	LDREX   r1, [r0]
	CMP     r1, #0
	STREXEQ r1, r2, [r0]
	MOV	r0, r1
	MCR     p15, 0, r0, c7, c10, 4
	bx      lr
	ENDFUNC

	ALIGN   4

;--------------------------------------------------------
; spin_unlock
;--------------------------------------------------------
spin_unlock FUNCTION
	MOV	r1, #0
	STR	r1, [r0]
	MCR	p15, 0, r0, c7, c10, 4
	bx	lr
	ENDFUNC

	ALIGN  4

;--------------------------------------------------------
; panic
;--------------------------------------------------------
panic FUNCTION
	isb
	dsb
	CPSID  aif
	B      panic
	ENDFUNC

;--------------------------------------------------------------
; Utility function that takes a pointer (r0), stack size (r1).
; It returns the pointer to the stack offset for the asked cpu
;--------------------------------------------------------------
get_sp FUNCTION
	ldr	r2, =0x2c001800
	ldr	r2, [r2]
	and	r2, r2, #0xff
	clz	r2, r2
	mov	r3, #32
	sub	r2, r3, r2
	mul	r2, r2, r1
	add	r0, r0, r2
	bx	lr
	ENDFUNC

disable_coherency FUNCTION
	push	{lr}
	bl      read_actlr
	bic	r0, r0, #0x40
	bl      write_actlr
	dsb
	isb
	pop	{lr}
	bx      lr
	ENDFUNC

enable_coherency FUNCTION
	push	{lr}
	bl      read_actlr
	orr	r0, r0, #0x40
	bl      write_actlr
	dsb
	isb
	pop	{lr}
	bx      lr
	ENDFUNC

inv_bpred_is FUNCTION
	mcr	p15, 0, r0, c7, c1, 6
	bx	lr
	ENDFUNC

inv_bpred_all FUNCTION
	mcr	p15, 0, r0, c7, c5, 6
	bx	lr
	ENDFUNC

inv_tlb_all FUNCTION
	mcr     p15, 0, r0, c8, c7, 0
	dsb
	isb
	bx	lr
	ENDFUNC

inv_tlb_mva FUNCTION
	mcr     p15, 0, r0, c8, c7, 1
	dsb
	isb
	bx	lr
	ENDFUNC

inv_icache_all FUNCTION
	mcr     p15, 0, r10, c7, c5, 0      ; invalidate I cache
	dsb
	isb
	bx	lr
	ENDFUNC

inv_icache_mva_pou FUNCTION
	mcr	p15, 0, r0, c7, c5, 1
	dsb
	isb
	bx	lr
	ENDFUNC

cln_dcache_mva_pou FUNCTION
	mcr	p15, 0, r0, c7, c11, 1
	dsb
	isb
	bx	lr
	ENDFUNC

cln_dcache_mva_poc FUNCTION
	mcr	p15, 0, r0, c7, c10, 1
	dsb
	isb
	bx	lr
	ENDFUNC

inv_dcache_mva_poc FUNCTION
	mcr	p15, 0, r0, c7, c6, 1
	dsb
	isb
	bx	lr
	ENDFUNC

	; Clean/Invalidate/Clean and invalidate a specified cache level.
	; Ignore if the level does not exist.
cache_maint_op FUNCTION
	push	{r4-r11}
	dsb
        lsl     r10, r0, #1	                ; start clean at specified cache level
        mrc     p15, 1, r0, c0, c0, 1           ; read clidr
10
        add     r2, r10, r10, lsr #1            ; work out 3x current cache level
        mov     r3, r0, lsr r2                  ; extract cache type bits from clidr
        and     r3, r3, #7                      ; mask of the bits for current cache only
        cmp     r3, #2                          ; see what cache we have at this level
        blt     %f50                            ; skip if no cache, or just i-cache
        mcr     p15, 2, r10, c0, c0, 0          ; select current cache level in cssr
        isb                                     ; isb to sych the new cssr&csidr
        mrc     p15, 1, r3, c0, c0, 0           ; read the new csidr
        and     r2, r3, #7                      ; extract the length of the cache lines
        add     r2, r2, #4                      ; add 4 (line length offset)
        ldr     r4, =0x3ff
        ands    r4, r4, r3, lsr #3              ; find maximum number on the way size
        clz     r5, r4                          ; find bit position of way size increment
        ldr     r7, =0x7fff
        ands    r7, r7, r3, lsr #13             ; extract max number of the index size
20
        mov     r9, r4                          ; create working copy of max way size
30
	orr     r11, r10, r9, lsl r5            ; factor way and cache number into r11
	lsl     r6, r9, r5
	orr     r11, r10, r6                    ; factor way and cache number into r11
	orr     r11, r11, r7, lsl r2            ; factor index number into r11
	lsl     r6, r7, r2
	orr     r11, r11, r6			; factor index number into r11
	cmp	r1, #INV
        mcreq   p15, 0, r11, c7, c6, 2          ; invalidate by set/way
	beq	%f40
	cmp	r1, #CLN
        mcreq   p15, 0, r11, c7, c10, 2         ; clean by set/way
	beq	%f40
        mcr     p15, 0, r11, c7, c14, 2         ; clean & invalidate by set/way
;	nop					; nop
40
        subs    r9, r9, #1                      ; decrement the way
        bge     %b30
        subs    r7, r7, #1                      ; decrement the index
        bge     %b20
50
        mov     r10, #0                         ; swith back to cache level 0
        mcr     p15, 2, r10, c0, c0, 0          ; select current cache level in cssr
        dsb
        isb
	pop     {r4-r11}
	bx      lr
	ENDFUNC

smc	FUNCTION
	push	{r4-r12, lr}
	smc	#0
	pop	{r4-r12, pc}
	ENDFUNC

hyp_save	FUNCTION
	hvc	#2
	bx	lr
	ENDFUNC

virt_memcpy	FUNCTION
	cmp	r2, #0
	bxeq	lr
0	ldrb	r3, [r1], #1
	strb	r3, [r0], #1
	subs	r2, #1
	bne	%b0
	bx	lr
	ENDFUNC

virt_memset	FUNCTION
	cmp	r2, #0
	bxeq	lr
0	strb	r1, [r0], #1
	subs	r2, #1
	bne	%b0
	bx	lr
	ENDFUNC

virt_dead	FUNCTION
	b	virt_dead
	ENDFUNC

num_secondaries FUNCTION
	mrc	p15, 1, r0, c9, c0, 2
	lsr	r0, r0, #24
	and	r0, r0, #3
	bx	lr
	ENDFUNC

dcisw FUNCTION
	mcr     p15, 0, r0, c7, c6, 2
	bx      lr
	ENDFUNC

dccsw FUNCTION
	mcr     p15, 0, r0, c7, c10, 2
	bx      lr
	ENDFUNC

dccisw FUNCTION
	mcr     p15, 0, r0, c7, c14, 2
	bx      lr
	ENDFUNC


	END