Orange Pi5 kernel

^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   1) /* SPDX-License-Identifier: GPL-2.0-only */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   2) /*
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   3)  *  linux/arch/arm/kernel/iwmmxt.S
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   4)  *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   5)  *  XScale iWMMXt (Concan) context switching and handling
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   6)  *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   7)  *  Initial code:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   8)  *  Copyright (c) 2003, Intel Corporation
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   9)  *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  10)  *  Full lazy switching support, optimizations and more, by Nicolas Pitre
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  11) *   Copyright (c) 2003-2004, MontaVista Software, Inc.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  12)  */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  13) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  14) #include <linux/linkage.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  15) #include <asm/ptrace.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  16) #include <asm/thread_info.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  17) #include <asm/asm-offsets.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  18) #include <asm/assembler.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  19) #include "iwmmxt.h"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  20) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  21) #if defined(CONFIG_CPU_PJ4) || defined(CONFIG_CPU_PJ4B)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  22) #define PJ4(code...)		code
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  23) #define XSC(code...)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  24) #elif defined(CONFIG_CPU_MOHAWK) || \
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  25) 	defined(CONFIG_CPU_XSC3) || \
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  26) 	defined(CONFIG_CPU_XSCALE)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  27) #define PJ4(code...)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  28) #define XSC(code...)		code
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  29) #else
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  30) #error "Unsupported iWMMXt architecture"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  31) #endif
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  32) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  33) #define MMX_WR0		 	(0x00)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  34) #define MMX_WR1		 	(0x08)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  35) #define MMX_WR2		 	(0x10)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  36) #define MMX_WR3			(0x18)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  37) #define MMX_WR4		 	(0x20)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  38) #define MMX_WR5		 	(0x28)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  39) #define MMX_WR6		 	(0x30)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  40) #define MMX_WR7		 	(0x38)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  41) #define MMX_WR8		 	(0x40)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  42) #define MMX_WR9		 	(0x48)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  43) #define MMX_WR10		(0x50)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  44) #define MMX_WR11		(0x58)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  45) #define MMX_WR12		(0x60)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  46) #define MMX_WR13		(0x68)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  47) #define MMX_WR14		(0x70)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  48) #define MMX_WR15		(0x78)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  49) #define MMX_WCSSF		(0x80)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  50) #define MMX_WCASF		(0x84)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  51) #define MMX_WCGR0		(0x88)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  52) #define MMX_WCGR1		(0x8C)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  53) #define MMX_WCGR2		(0x90)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  54) #define MMX_WCGR3		(0x94)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  55) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  56) #define MMX_SIZE		(0x98)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  57) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  58) 	.text
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  59) 	.arm
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  60) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  61) /*
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  62)  * Lazy switching of Concan coprocessor context
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  63)  *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  64)  * r10 = struct thread_info pointer
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  65)  * r9  = ret_from_exception
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  66)  * lr  = undefined instr exit
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  67)  *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  68)  * called from prefetch exception handler with interrupts enabled
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  69)  */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  70) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  71) ENTRY(iwmmxt_task_enable)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  72) 	inc_preempt_count r10, r3
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  73) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  74) 	XSC(mrc	p15, 0, r2, c15, c1, 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  75) 	PJ4(mrc p15, 0, r2, c1, c0, 2)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  76) 	@ CP0 and CP1 accessible?
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  77) 	XSC(tst	r2, #0x3)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  78) 	PJ4(tst	r2, #0xf)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  79) 	bne	4f				@ if so no business here
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  80) 	@ enable access to CP0 and CP1
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  81) 	XSC(orr	r2, r2, #0x3)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  82) 	XSC(mcr	p15, 0, r2, c15, c1, 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  83) 	PJ4(orr	r2, r2, #0xf)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  84) 	PJ4(mcr	p15, 0, r2, c1, c0, 2)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  85) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  86) 	ldr	r3, =concan_owner
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  87) 	add	r0, r10, #TI_IWMMXT_STATE	@ get task Concan save area
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  88) 	ldr	r2, [sp, #60]			@ current task pc value
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  89) 	ldr	r1, [r3]			@ get current Concan owner
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  90) 	str	r0, [r3]			@ this task now owns Concan regs
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  91) 	sub	r2, r2, #4			@ adjust pc back
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  92) 	str	r2, [sp, #60]
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  93) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  94) 	mrc	p15, 0, r2, c2, c0, 0
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  95) 	mov	r2, r2				@ cpwait
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  96) 	bl	concan_save
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  97) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  98) #ifdef CONFIG_PREEMPT_COUNT
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  99) 	get_thread_info r10
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 100) #endif
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 101) 4:	dec_preempt_count r10, r3
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 102) 	ret	r9				@ normal exit from exception
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 103) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 104) concan_save:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 105) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 106) 	teq	r1, #0				@ test for last ownership
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 107) 	beq	concan_load			@ no owner, skip save
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 108) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 109) 	tmrc	r2, wCon
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 110) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 111) 	@ CUP? wCx
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 112) 	tst	r2, #0x1
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 113) 	beq 	1f
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 114) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 115) concan_dump:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 116) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 117) 	wstrw	wCSSF, r1, MMX_WCSSF
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 118) 	wstrw	wCASF, r1, MMX_WCASF
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 119) 	wstrw	wCGR0, r1, MMX_WCGR0
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 120) 	wstrw	wCGR1, r1, MMX_WCGR1
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 121) 	wstrw	wCGR2, r1, MMX_WCGR2
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 122) 	wstrw	wCGR3, r1, MMX_WCGR3
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 123) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 124) 1:	@ MUP? wRn
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 125) 	tst	r2, #0x2
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 126) 	beq	2f
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 127) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 128) 	wstrd	wR0,  r1, MMX_WR0
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 129) 	wstrd	wR1,  r1, MMX_WR1
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 130) 	wstrd	wR2,  r1, MMX_WR2
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 131) 	wstrd	wR3,  r1, MMX_WR3
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 132) 	wstrd	wR4,  r1, MMX_WR4
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 133) 	wstrd	wR5,  r1, MMX_WR5
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 134) 	wstrd	wR6,  r1, MMX_WR6
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 135) 	wstrd	wR7,  r1, MMX_WR7
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 136) 	wstrd	wR8,  r1, MMX_WR8
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 137) 	wstrd	wR9,  r1, MMX_WR9
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 138) 	wstrd	wR10, r1, MMX_WR10
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 139) 	wstrd	wR11, r1, MMX_WR11
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 140) 	wstrd	wR12, r1, MMX_WR12
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 141) 	wstrd	wR13, r1, MMX_WR13
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 142) 	wstrd	wR14, r1, MMX_WR14
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 143) 	wstrd	wR15, r1, MMX_WR15
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 144) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 145) 2:	teq	r0, #0				@ anything to load?
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 146) 	reteq	lr				@ if not, return
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 147) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 148) concan_load:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 149) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 150) 	@ Load wRn
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 151) 	wldrd	wR0,  r0, MMX_WR0
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 152) 	wldrd	wR1,  r0, MMX_WR1
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 153) 	wldrd	wR2,  r0, MMX_WR2
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 154) 	wldrd	wR3,  r0, MMX_WR3
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 155) 	wldrd	wR4,  r0, MMX_WR4
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 156) 	wldrd	wR5,  r0, MMX_WR5
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 157) 	wldrd	wR6,  r0, MMX_WR6
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 158) 	wldrd	wR7,  r0, MMX_WR7
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 159) 	wldrd	wR8,  r0, MMX_WR8
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 160) 	wldrd	wR9,  r0, MMX_WR9
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 161) 	wldrd	wR10, r0, MMX_WR10
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 162) 	wldrd	wR11, r0, MMX_WR11
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 163) 	wldrd	wR12, r0, MMX_WR12
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 164) 	wldrd	wR13, r0, MMX_WR13
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 165) 	wldrd	wR14, r0, MMX_WR14
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 166) 	wldrd	wR15, r0, MMX_WR15
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 167) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 168) 	@ Load wCx
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 169) 	wldrw	wCSSF, r0, MMX_WCSSF
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 170) 	wldrw	wCASF, r0, MMX_WCASF
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 171) 	wldrw	wCGR0, r0, MMX_WCGR0
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 172) 	wldrw	wCGR1, r0, MMX_WCGR1
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 173) 	wldrw	wCGR2, r0, MMX_WCGR2
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 174) 	wldrw	wCGR3, r0, MMX_WCGR3
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 175) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 176) 	@ clear CUP/MUP (only if r1 != 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 177) 	teq	r1, #0
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 178) 	mov 	r2, #0
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 179) 	reteq	lr
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 180) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 181) 	tmcr	wCon, r2
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 182) 	ret	lr
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 183) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 184) ENDPROC(iwmmxt_task_enable)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 185) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 186) /*
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 187)  * Back up Concan regs to save area and disable access to them
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 188)  * (mainly for gdb or sleep mode usage)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 189)  *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 190)  * r0 = struct thread_info pointer of target task or NULL for any
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 191)  */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 192) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 193) ENTRY(iwmmxt_task_disable)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 194) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 195) 	stmfd	sp!, {r4, lr}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 196) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 197) 	mrs	ip, cpsr
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 198) 	orr	r2, ip, #PSR_I_BIT		@ disable interrupts
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 199) 	msr	cpsr_c, r2
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 200) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 201) 	ldr	r3, =concan_owner
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 202) 	add	r2, r0, #TI_IWMMXT_STATE	@ get task Concan save area
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 203) 	ldr	r1, [r3]			@ get current Concan owner
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 204) 	teq	r1, #0				@ any current owner?
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 205) 	beq	1f				@ no: quit
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 206) 	teq	r0, #0				@ any owner?
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 207) 	teqne	r1, r2				@ or specified one?
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 208) 	bne	1f				@ no: quit
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 209) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 210) 	@ enable access to CP0 and CP1
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 211) 	XSC(mrc	p15, 0, r4, c15, c1, 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 212) 	XSC(orr	r4, r4, #0x3)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 213) 	XSC(mcr	p15, 0, r4, c15, c1, 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 214) 	PJ4(mrc p15, 0, r4, c1, c0, 2)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 215) 	PJ4(orr	r4, r4, #0xf)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 216) 	PJ4(mcr	p15, 0, r4, c1, c0, 2)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 217) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 218) 	mov	r0, #0				@ nothing to load
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 219) 	str	r0, [r3]			@ no more current owner
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 220) 	mrc	p15, 0, r2, c2, c0, 0
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 221) 	mov	r2, r2				@ cpwait
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 222) 	bl	concan_save
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 223) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 224) 	@ disable access to CP0 and CP1
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 225) 	XSC(bic	r4, r4, #0x3)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 226) 	XSC(mcr	p15, 0, r4, c15, c1, 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 227) 	PJ4(bic	r4, r4, #0xf)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 228) 	PJ4(mcr	p15, 0, r4, c1, c0, 2)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 229) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 230) 	mrc	p15, 0, r2, c2, c0, 0
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 231) 	mov	r2, r2				@ cpwait
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 232) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 233) 1:	msr	cpsr_c, ip			@ restore interrupt mode
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 234) 	ldmfd	sp!, {r4, pc}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 235) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 236) ENDPROC(iwmmxt_task_disable)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 237) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 238) /*
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 239)  * Copy Concan state to given memory address
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 240)  *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 241)  * r0 = struct thread_info pointer of target task
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 242)  * r1 = memory address where to store Concan state
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 243)  *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 244)  * this is called mainly in the creation of signal stack frames
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 245)  */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 246) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 247) ENTRY(iwmmxt_task_copy)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 248) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 249) 	mrs	ip, cpsr
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 250) 	orr	r2, ip, #PSR_I_BIT		@ disable interrupts
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 251) 	msr	cpsr_c, r2
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 252) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 253) 	ldr	r3, =concan_owner
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 254) 	add	r2, r0, #TI_IWMMXT_STATE	@ get task Concan save area
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 255) 	ldr	r3, [r3]			@ get current Concan owner
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 256) 	teq	r2, r3				@ does this task own it...
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 257) 	beq	1f
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 258) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 259) 	@ current Concan values are in the task save area
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 260) 	msr	cpsr_c, ip			@ restore interrupt mode
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 261) 	mov	r0, r1
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 262) 	mov	r1, r2
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 263) 	mov	r2, #MMX_SIZE
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 264) 	b	memcpy
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 265) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 266) 1:	@ this task owns Concan regs -- grab a copy from there
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 267) 	mov	r0, #0				@ nothing to load
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 268) 	mov	r2, #3				@ save all regs
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 269) 	mov	r3, lr				@ preserve return address
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 270) 	bl	concan_dump
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 271) 	msr	cpsr_c, ip			@ restore interrupt mode
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 272) 	ret	r3
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 273) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 274) ENDPROC(iwmmxt_task_copy)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 275) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 276) /*
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 277)  * Restore Concan state from given memory address
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 278)  *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 279)  * r0 = struct thread_info pointer of target task
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 280)  * r1 = memory address where to get Concan state from
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 281)  *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 282)  * this is used to restore Concan state when unwinding a signal stack frame
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 283)  */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 284) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 285) ENTRY(iwmmxt_task_restore)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 286) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 287) 	mrs	ip, cpsr
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 288) 	orr	r2, ip, #PSR_I_BIT		@ disable interrupts
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 289) 	msr	cpsr_c, r2
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 290) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 291) 	ldr	r3, =concan_owner
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 292) 	add	r2, r0, #TI_IWMMXT_STATE	@ get task Concan save area
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 293) 	ldr	r3, [r3]			@ get current Concan owner
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 294) 	bic	r2, r2, #0x7			@ 64-bit alignment
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 295) 	teq	r2, r3				@ does this task own it...
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 296) 	beq	1f
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 297) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 298) 	@ this task doesn't own Concan regs -- use its save area
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 299) 	msr	cpsr_c, ip			@ restore interrupt mode
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 300) 	mov	r0, r2
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 301) 	mov	r2, #MMX_SIZE
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 302) 	b	memcpy
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 303) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 304) 1:	@ this task owns Concan regs -- load them directly
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 305) 	mov	r0, r1
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 306) 	mov	r1, #0				@ don't clear CUP/MUP
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 307) 	mov	r3, lr				@ preserve return address
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 308) 	bl	concan_load
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 309) 	msr	cpsr_c, ip			@ restore interrupt mode
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 310) 	ret	r3
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 311) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 312) ENDPROC(iwmmxt_task_restore)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 313) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 314) /*
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 315)  * Concan handling on task switch
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 316)  *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 317)  * r0 = next thread_info pointer
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 318)  *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 319)  * Called only from the iwmmxt notifier with task preemption disabled.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 320)  */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 321) ENTRY(iwmmxt_task_switch)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 322) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 323) 	XSC(mrc	p15, 0, r1, c15, c1, 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 324) 	PJ4(mrc	p15, 0, r1, c1, c0, 2)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 325) 	@ CP0 and CP1 accessible?
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 326) 	XSC(tst	r1, #0x3)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 327) 	PJ4(tst	r1, #0xf)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 328) 	bne	1f				@ yes: block them for next task
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 329) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 330) 	ldr	r2, =concan_owner
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 331) 	add	r3, r0, #TI_IWMMXT_STATE	@ get next task Concan save area
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 332) 	ldr	r2, [r2]			@ get current Concan owner
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 333) 	teq	r2, r3				@ next task owns it?
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 334) 	retne	lr				@ no: leave Concan disabled
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 335) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 336) 1:	@ flip Concan access
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 337) 	XSC(eor	r1, r1, #0x3)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 338) 	XSC(mcr	p15, 0, r1, c15, c1, 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 339) 	PJ4(eor r1, r1, #0xf)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 340) 	PJ4(mcr	p15, 0, r1, c1, c0, 2)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 341) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 342) 	mrc	p15, 0, r1, c2, c0, 0
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 343) 	sub	pc, lr, r1, lsr #32		@ cpwait and return
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 344) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 345) ENDPROC(iwmmxt_task_switch)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 346) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 347) /*
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 348)  * Remove Concan ownership of given task
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 349)  *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 350)  * r0 = struct thread_info pointer
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 351)  */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 352) ENTRY(iwmmxt_task_release)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 353) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 354) 	mrs	r2, cpsr
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 355) 	orr	ip, r2, #PSR_I_BIT		@ disable interrupts
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 356) 	msr	cpsr_c, ip
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 357) 	ldr	r3, =concan_owner
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 358) 	add	r0, r0, #TI_IWMMXT_STATE	@ get task Concan save area
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 359) 	ldr	r1, [r3]			@ get current Concan owner
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 360) 	eors	r0, r0, r1			@ if equal...
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 361) 	streq	r0, [r3]			@ then clear ownership
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 362) 	msr	cpsr_c, r2			@ restore interrupts
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 363) 	ret	lr
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 364) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 365) ENDPROC(iwmmxt_task_release)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 366) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 367) 	.data
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 368) 	.align	2
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 369) concan_owner:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 370) 	.word	0
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 371)