sun7i: PSCI enhancements

index b9ea78b..a84807d 100644
@@ -68,7 +68,7 @@ psci_arch_init:
 
 	mrc	p15, 0, r4, c0, c0, 5	@ MPIDR
 	and	r4, r4, #3		@ cpu number in cluster
-	mov	r5, #400		@ 1kB of stack per CPU
+	mov	r5, #0x400		@ 1kB of stack per CPU
 	mul	r4, r4, r5
 
 	adr	r5, text_end		@ end of text

index a84807d..0aa4007 100644
@@ -18,6 +18,7 @@
  */
 
 #include <config.h>
+#include <asm/gic.h>
 #include <asm/psci.h>
 #include <asm/arch/cpu.h>
 

@@ -38,6 +39,8 @@
 
 #define	ONE_MS			(CONFIG_SYS_CLK_FREQ / 1000)
 #define	TEN_MS			(10 * ONE_MS)
+#define	GICD_BASE		0x1c81000
+#define	GICC_BASE		0x1c82000
 
 .macro	timer_wait	reg, ticks
 	@ Program CNTP_TVAL

@@ -61,7 +64,27 @@
 
 .globl	psci_arch_init
 psci_arch_init:
+	movw	r4, #(GICD_BASE & 0xffff)
+	movt	r4, #(GICD_BASE >> 16)
+
+	ldr	r5, [r4, #GICD_IGROUPRn]
+	bic	r5, r5, #(1 << 15) 	@ SGI15 as Group-0
+	str	r5, [r4, #GICD_IGROUPRn]
+
+	mov	r5, #0			@ Set SGI15 priority to 0
+	strb	r5, [r4, #(GICD_IPRIORITYRn + 15)]
+
+	add	r4, r4, #0x1000		@ GICC address
+
+	mov	r5, #0xff
+	str	r5, [r4, #GICC_PMR]	@ Be cool with non-secure
+
+	ldr	r5, [r4, #GICC_CTLR]
+	orr	r5, r5, #(1 << 3)	@ Switch FIQEn on
+	str	r5, [r4, #GICC_CTLR]
+
 	mrc	p15, 0, r5, c1, c1, 0	@ Read SCR
+	orr	r5, r5, #4		@ Enable FIQ in monitor mode
 	bic	r5, r5, #1		@ Secure mode
 	mcr	p15, 0, r5, c1, c1, 0	@ Write SCR
 	isb

@@ -79,6 +102,77 @@ psci_arch_init:
 
 	bx	lr
 
+.globl	psci_fiq_enter
+psci_fiq_enter:
+	push	{r0-r12}
+
+	@ Switch to secure
+	mrc	p15, 0, r7, c1, c1, 0
+	bic	r8, r7, #1
+	mcr	p15, 0, r8, c1, c1, 0
+	isb
+
+	movw	r8, #(GICC_BASE & 0xffff)
+	movt	r8, #(GICC_BASE >> 16)
+	ldr	r9, [r8, #GICC_IAR]
+	movw	r10, #0x3ff
+	movt	r10, #0
+	cmp	r9, r10
+	beq	out
+	movw	r10, #0x3fe
+	cmp	r9, r10
+	beq	out
+	str	r9, [r8, #GICC_EOIR]
+	dsb
+
+	@ Compute CPU number
+	lsr	r9, r9, #10
+	and	r9, r9, #0xf
+
+	movw	r8, #(SUN7I_CPUCFG_BASE & 0xffff)
+	movt	r8, #(SUN7I_CPUCFG_BASE >> 16)
+
+	@ Wait for the core to enter WFI
+	lsl	r11, r9, #6		@ x64
+	add	r11, r11, r8
+
+1:	ldr	r10, [r11, #0x48]
+	tst	r10, #(1 << 2)
+	bne	2f
+	timer_wait r10, ONE_MS
+	b	1b
+
+	@ Reset CPU
+2:	mov	r10, #0
+	str	r10, [r11, #0x40]
+
+	@ Lock CPU
+	mov	r10, #1
+	lsl	r9, r10, r9		@ r9 is now CPU mask
+	ldr	r10, [r8, #0x1e4]
+	bic	r10, r10, r9
+	str	r10, [r8, #0x1e4]
+
+	@ Set power gating
+	ldr	r10, [r8, #0x1b4]
+	orr	r10, r10, #1
+	str	r10, [r8, #0x1b4]
+	timer_wait r10, ONE_MS
+
+	@ Activate power clamp
+	mov	r10, #1
+1:	str	r10, [r8, #0x1b0]
+	lsl	r10, r10, #1
+	orr	r10, r10, #1
+	tst	r10, #0x100
+	beq	1b
+
+	@ Restore security level
+out:	mcr	p15, 0, r7, c1, c1, 0
+
+	pop	{r0-r12}
+	subs    pc, lr, #4
+
 	@ r1 = target CPU
 	@ r2 = target PC
 .globl	psci_cpu_on

@@ -144,6 +238,53 @@ psci_cpu_on:
 _target_pc:
 	.word	0
 
+/* Imported from Linux kernel */
+v7_flush_dcache_all:
+	dmb					@ ensure ordering with previous memory accesses
+	mrc	p15, 1, r0, c0, c0, 1		@ read clidr
+	ands	r3, r0, #0x7000000		@ extract loc from clidr
+	mov	r3, r3, lsr #23			@ left align loc bit field
+	beq	finished			@ if loc is 0, then no need to clean
+	mov	r10, #0				@ start clean at cache level 0
+flush_levels:
+	add	r2, r10, r10, lsr #1		@ work out 3x current cache level
+	mov	r1, r0, lsr r2			@ extract cache type bits from clidr
+	and	r1, r1, #7			@ mask of the bits for current cache only
+	cmp	r1, #2				@ see what cache we have@this level
+	blt	skip				@ skip if no cache, or just i-cache
+	mrs     r9, cpsr			@ make cssr&csidr read atomic
+	mcr	p15, 2, r10, c0, c0, 0		@ select current cache level in cssr
+	isb					@ isb to sych the new cssr&csidr
+	mrc	p15, 1, r1, c0, c0, 0		@ read the new csidr
+	msr     cpsr_c, r9
+	and	r2, r1, #7			@ extract the length of the cache lines
+	add	r2, r2, #4			@ add 4 (line length offset)
+	ldr	r4, =0x3ff
+	ands	r4, r4, r1, 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, r1, lsr #13		@ extract max number of the index size
+loop1:
+	mov	r9, r7				@ create working copy of max index
+loop2:
+	orr	r11, r10, r4, lsl r5		@ factor way and cache number into r11
+	orr	r11, r11, r9, lsl r2		@ factor index number into r11
+	mcr	p15, 0, r11, c7, c14, 2		@ clean & invalidate by set/way
+	subs	r9, r9, #1			@ decrement the index
+	bge	loop2
+	subs	r4, r4, #1			@ decrement the way
+	bge	loop1
+skip:
+	add	r10, r10, #2			@ increment cache number
+	cmp	r3, r10
+	bgt	flush_levels
+finished:
+	mov	r10, #0				@ swith back to cache level 0
+	mcr	p15, 2, r10, c0, c0, 0		@ select current cache level in cssr
+	dsb	st
+	isb
+	bx	lr
+
 _sunxi_cpu_entry:
 	@ Set SMP bit
 	mrc	p15, 0, r0, c1, c0, 1

@@ -158,5 +299,34 @@ _sunxi_cpu_entry:
 	ldr	r0, [r0]
 	b	_do_nonsec_entry
 
+.globl	psci_cpu_off
+psci_cpu_off:
+	mrc	p15, 0, r0, c1, c0, 0		@ SCTLR
+	bic	r0, r0, #(1 << 2)		@ Clear C bit
+	mcr	p15, 0, r0, c1, c0, 0		@ SCTLR
+	isb
+	dsb
+
+	bl	v7_flush_dcache_all
+
+	clrex					@ Why???
+
+	mrc	p15, 0, r0, c1, c0, 1		@ ACTLR
+	bic	r0, r0, #(1 << 6)		@ Clear SMP bit
+	mcr	p15, 0, r0, c1, c0, 1		@ ACTLR
+	isb
+	dsb
+
+	@ Ask CPU0 to pull the rug...
+	movw	r0, #(GICD_BASE & 0xffff)
+	movt	r0, #(GICD_BASE >> 16)
+	movw	r1, #15				@ SGI15
+	movt	r1, #1				@ Target is CPU0
+	str	r1, [r0, #GICD_SGIR]
+	dsb
+
+1:	wfi
+	b	1b
+
 text_end:
 	.popsection

index bf11a34..e2a38ca 100644
@@ -49,8 +49,18 @@ ENTRY(psci_cpu_suspend)
 ENTRY(psci_cpu_off)
 ENTRY(psci_cpu_on)
 ENTRY(psci_migrate)
+ENTRY(psci_affinity_info)
+ENTRY(psci_migrate_info_type)
+ENTRY(psci_migrate_info_up_cpu)
+ENTRY(psci_system_off)
+ENTRY(psci_system_reset)
 	mov	r0, #ARM_PSCI_RET_NI	@ Return -1 (Not Implemented)
 	mov	pc, lr
+ENDPROC(psci_system_reset)
+ENDPROC(psci_system_off)
+ENDPROC(psci_migrate_info_up_cpu)
+ENDPROC(psci_migrate_info_type)
+ENDPROC(psci_affinity_info)
 ENDPROC(psci_migrate)
 ENDPROC(psci_cpu_on)
 ENDPROC(psci_cpu_off)

@@ -59,16 +69,33 @@ ENDPROC(psci_cpu_suspend)
 .weak psci_cpu_off
 .weak psci_cpu_on
 .weak psci_migrate
+.weak psci_affinity_info
+.weak psci_migrate_info_type
+.weak psci_migrate_info_up_cpu
+.weak psci_system_off
+.weak psci_system_reset
 
 _psci_table:
+	.word	ARM_PSCI_FN_PSCI_VERSION
+	.word	psci_version
 	.word	ARM_PSCI_FN_CPU_SUSPEND
 	.word	psci_cpu_suspend
 	.word	ARM_PSCI_FN_CPU_OFF
 	.word	psci_cpu_off
 	.word	ARM_PSCI_FN_CPU_ON
 	.word	psci_cpu_on
+	.word	ARM_PSCI_FN_AFFINITY_INFO
+	.word	psci_affinity_info
 	.word	ARM_PSCI_FN_MIGRATE
 	.word	psci_migrate
+	.word	ARM_PSCI_FN_MIGRATE_INFO_TYPE
+	.word	psci_migrate_info_type
+	.word	ARM_PSCI_FN_MIGRATE_INFO_UP_CPU
+	.word	psci_migrate_info_up_cpu
+	.word	ARM_PSCI_FN_SYSTEM_OFF
+	.word	psci_system_off
+	.word	ARM_PSCI_FN_SYSTEM_RESET
+	.word	psci_system_reset
 	.word	0
 	.word	0
 

@@ -86,7 +113,7 @@ _smc_psci:
 	ldr	r6, [r4, #4]		@ Load target PC
 	cmp	r5, #0			@ If reach the end, bail out
 	moveq	r0, #ARM_PSCI_RET_INVAL	@ Return -2 (Invalid)
-	beq	2f
+	beq	return
 	cmp	r0, r5			@ If not matching, try next entry
 	addne	r4, r4, #8
 	bne	1b

@@ -94,9 +121,13 @@ _smc_psci:
 	blx	r6			@ Execute PSCI function
 
 	@ Switch back to non-secure
-2:	mcr	p15, 0, r7, c1, c1, 0
+return:	mcr	p15, 0, r7, c1, c1, 0
 
 	pop	{r4-r7, lr}
 	movs	pc, lr			@ Return to the kernel
 
+psci_version:
+	mov	r0, #0x00000002		@ Version 0.2
+	b	return
+
 	.popsection

156 lines not on this page · raw message

index bba1894..a5b2b0c 100644
@@ -94,46 +94,6 @@
 	isb
 .endm
 
-.globl	psci_arch_init
-psci_arch_init:
-	movw	r4, #(GICD_BASE & 0xffff)
-	movt	r4, #(GICD_BASE >> 16)
-
-	ldr	r5, [r4, #GICD_IGROUPRn]
-	bic	r5, r5, #(1 << 15) 	@ SGI15 as Group-0
-	str	r5, [r4, #GICD_IGROUPRn]
-
-	mov	r5, #0			@ Set SGI15 priority to 0
-	strb	r5, [r4, #(GICD_IPRIORITYRn + 15)]
-
-	add	r4, r4, #0x1000		@ GICC address
-
-	mov	r5, #0xff
-	str	r5, [r4, #GICC_PMR]	@ Be cool with non-secure
-
-	ldr	r5, [r4, #GICC_CTLR]
-	orr	r5, r5, #(1 << 3)	@ Switch FIQEn on
-	str	r5, [r4, #GICC_CTLR]
-
-	mrc	p15, 0, r5, c1, c1, 0	@ Read SCR
-	orr	r5, r5, #4		@ Enable FIQ in monitor mode
-	bic	r5, r5, #1		@ Secure mode
-	mcr	p15, 0, r5, c1, c1, 0	@ Write SCR
-	isb
-
-	mrc	p15, 0, r4, c0, c0, 5	@ MPIDR
-	and	r4, r4, #3		@ cpu number in cluster
-	mov	r5, #0x400		@ 1kB of stack per CPU
-	mul	r4, r4, r5
-
-	adr	r5, text_end		@ end of text
-	add	r5, r5, #0x2000		@ Skip two pages
-	lsr	r5, r5, #12		@ Align to start of page
-	lsl	r5, r5, #12
-	sub	sp, r5, r4		@ here's our stack!
-
-	bx	lr
-
 .globl	psci_fiq_enter
 psci_fiq_enter:
 	push	{r0-r12}

@@ -445,5 +405,45 @@ psci_system_reset:
 	str	r1, [r0, #WDOG_CTL]
 1:	b	1b
 
+.globl	psci_arch_init
+psci_arch_init:
+	movw	r4, #(GICD_BASE & 0xffff)
+	movt	r4, #(GICD_BASE >> 16)
+
+	ldr	r5, [r4, #GICD_IGROUPRn]
+	bic	r5, r5, #(1 << 15) 	@ SGI15 as Group-0
+	str	r5, [r4, #GICD_IGROUPRn]
+
+	mov	r5, #0			@ Set SGI15 priority to 0
+	strb	r5, [r4, #(GICD_IPRIORITYRn + 15)]
+
+	add	r4, r4, #0x1000		@ GICC address
+
+	mov	r5, #0xff
+	str	r5, [r4, #GICC_PMR]	@ Be cool with non-secure
+
+	ldr	r5, [r4, #GICC_CTLR]
+	orr	r5, r5, #(1 << 3)	@ Switch FIQEn on
+	str	r5, [r4, #GICC_CTLR]
+
+	mrc	p15, 0, r5, c1, c1, 0	@ Read SCR
+	orr	r5, r5, #4		@ Enable FIQ in monitor mode
+	bic	r5, r5, #1		@ Secure mode
+	mcr	p15, 0, r5, c1, c1, 0	@ Write SCR
+	isb
+
+	mrc	p15, 0, r4, c0, c0, 5	@ MPIDR
+	and	r4, r4, #3		@ cpu number in cluster
+	mov	r5, #0x400		@ 1kB of stack per CPU
+	mul	r4, r4, r5
+
+	adr	r5, text_end		@ end of text
+	add	r5, r5, #0x2000		@ Skip two pages
+	lsr	r5, r5, #12		@ Align to start of page
+	lsl	r5, r5, #12
+	sub	sp, r5, r4		@ here's our stack!
+
+	bx	lr
+
 text_end:
 	.popsection

index a5b2b0c..e62e295 100644
@@ -159,6 +159,12 @@ psci_fiq_enter:
 	tst	r10, #0x100
 	beq	1b
 
+	@ Mark CPU disabled
+	adr	r2, _cpu_off
+	ldr	r0, [r2]
+	orr	r0, r0, r9
+	str	r0, [r2]
+
 	@ Restore security level
 out:	mcr	p15, 0, r7, c1, c1, 0
 

@@ -227,6 +233,12 @@ psci_cpu_on:
 	orr	r6, r6, r4
 	str	r6, [r0, #0x1e4]
 
+	@ Mark CPU enabled
+	adr	r2, _cpu_off
+	ldr	r0, [r2]
+	bic	r0, r0, r4
+	str	r0, [r2]
+
 	mov	r0, #ARM_PSCI_RET_SUCCESS	@ Return PSCI_RET_SUCCESS
 	mov	pc, lr
 

@@ -234,6 +246,8 @@ _target_pc:
 	.word	0
 _context:
 	.word	0
+_cpu_off:
+	.word	0x2
 
 /* Imported from Linux kernel */
 v7_flush_dcache_all:

@@ -327,6 +341,25 @@ psci_cpu_off:
 1:	wfi
 	b	1b
 
+	@ r1 = target CPU
+	@ r2 = lowest affinity level
+.globl	psci_affinity_info
+psci_affinity_info:
+	@ only support full match
+	cmp	r2, #0
+	mov	r0, #ARM_PSCI_RET_INVAL
+	bne	1
+
+	@ prepare CPU ID in r1, only caring about the first cluster
+	and	r1, r1, #3
+
+	adr	r2, _cpu_off
+	ldr	r0, [r2]			@ load bitmap of offlined CPUs
+	lsr	r0, r0, r1			@ shift right by CPU ID
+	and	r0, r0, #1			@ filter out relevant bit
+
+1:	mov	pc, lr
+
 /*
  * r0: TWI base address
  * r1: state to wait for