ARMv7: Factor out reusable timer_wait from sunxi/psci_sun7i.S

1/2

index d4cb51e..4ff46e4 100644
@@ -18,6 +18,8 @@
  */
 
 #include <config.h>
+
+#include <asm/arch-armv7/generictimer.h>
 #include <asm/gic.h>
 #include <asm/macro.h>
 #include <asm/psci.h>

@@ -43,26 +45,6 @@
 #define	GICD_BASE		0x1c81000
 #define	GICC_BASE		0x1c82000
 
-.macro	timer_wait	reg, ticks
-	@ Program CNTP_TVAL
-	movw	\reg, #(\ticks & 0xffff)
-	movt	\reg, #(\ticks >> 16)
-	mcr	p15, 0, \reg, c14, c2, 0
-	isb
-	@ Enable physical timer, mask interrupt
-	mov	\reg, #3
-	mcr	p15, 0, \reg, c14, c2, 1
-	@ Poll physical timer until ISTATUS is on
-1:	isb
-	mrc	p15, 0, \reg, c14, c2, 1
-	ands	\reg, \reg, #4
-	bne	1b
-	@ Disable timer
-	mov	\reg, #0
-	mcr	p15, 0, \reg, c14, c2, 1
-	isb
-.endm
-
 .globl	psci_fiq_enter
 psci_fiq_enter:
 	push	{r0-r12}

index bbfeec8..e15d587 100644
@@ -18,6 +18,8 @@
  */
 
 #include <config.h>
+
+#include <asm/arch-armv7/generictimer.h>
 #include <asm/gic.h>
 #include <asm/macro.h>
 #include <asm/psci.h>

@@ -43,26 +45,6 @@
 #define	GICD_BASE		0x1c81000
 #define	GICC_BASE		0x1c82000
 
-.macro	timer_wait	reg, ticks
-	@ Program CNTP_TVAL
-	movw	\reg, #(\ticks & 0xffff)
-	movt	\reg, #(\ticks >> 16)
-	mcr	p15, 0, \reg, c14, c2, 0
-	isb
-	@ Enable physical timer, mask interrupt
-	mov	\reg, #3
-	mcr	p15, 0, \reg, c14, c2, 1
-	@ Poll physical timer until ISTATUS is on
-1:	isb
-	mrc	p15, 0, \reg, c14, c2, 1
-	ands	\reg, \reg, #4
-	bne	1b
-	@ Disable timer
-	mov	\reg, #0
-	mcr	p15, 0, \reg, c14, c2, 1
-	isb
-.endm
-
 .globl	psci_fiq_enter
 psci_fiq_enter:
 	push	{r0-r12}

2.1.0.27.g96db324
index d4cb51e..4ff46e4 100644
@@ -18,6 +18,8 @@
  */
 
 #include <config.h>
+
+#include <asm/arch-armv7/generictimer.h>
 #include <asm/gic.h>
 #include <asm/macro.h>
 #include <asm/psci.h>

@@ -43,26 +45,6 @@
 #define	GICD_BASE		0x1c81000
 #define	GICC_BASE		0x1c82000
 
-.macro	timer_wait	reg, ticks
-	@ Program CNTP_TVAL
-	movw	\reg, #(\ticks & 0xffff)
-	movt	\reg, #(\ticks >> 16)
-	mcr	p15, 0, \reg, c14, c2, 0
-	isb
-	@ Enable physical timer, mask interrupt
-	mov	\reg, #3
-	mcr	p15, 0, \reg, c14, c2, 1
-	@ Poll physical timer until ISTATUS is on
-1:	isb
-	mrc	p15, 0, \reg, c14, c2, 1
-	ands	\reg, \reg, #4
-	bne	1b
-	@ Disable timer
-	mov	\reg, #0
-	mcr	p15, 0, \reg, c14, c2, 1
-	isb
-.endm
-
 .globl	psci_fiq_enter
 psci_fiq_enter:
 	push	{r0-r12}

index bbfeec8..e15d587 100644
@@ -18,6 +18,8 @@
  */
 
 #include <config.h>
+
+#include <asm/arch-armv7/generictimer.h>
 #include <asm/gic.h>
 #include <asm/macro.h>
 #include <asm/psci.h>

@@ -43,26 +45,6 @@
 #define	GICD_BASE		0x1c81000
 #define	GICC_BASE		0x1c82000
 
-.macro	timer_wait	reg, ticks
-	@ Program CNTP_TVAL
-	movw	\reg, #(\ticks & 0xffff)
-	movt	\reg, #(\ticks >> 16)
-	mcr	p15, 0, \reg, c14, c2, 0
-	isb
-	@ Enable physical timer, mask interrupt
-	mov	\reg, #3
-	mcr	p15, 0, \reg, c14, c2, 1
-	@ Poll physical timer until ISTATUS is on
-1:	isb
-	mrc	p15, 0, \reg, c14, c2, 1
-	ands	\reg, \reg, #4
-	bne	1b
-	@ Disable timer
-	mov	\reg, #0
-	mcr	p15, 0, \reg, c14, c2, 1
-	isb
-.endm
-
 .globl	psci_fiq_enter
 psci_fiq_enter:
 	push	{r0-r12}

new file mode 100644
index 0000000..0956d7c
@@ -0,0 +1,50 @@
+/*
+ * Copyright (C) 2013 - ARM Ltd
+ * Author: Marc Zyngier <[email protected]>
+ *
+ * Based on code by Carl van Schaik <[email protected]>.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef _GENERICTIMER_H_
+#define _GENERICTIMER_H_
+
+#ifdef __ASSEMBLY__
+
+/*
+ * This macro provide a physical timer that can be used for delay in the code.
+ * The macro is moved from sunxi/psci_sun7i.S
+ *
+ * reg: is used in this macro.
+ * ticks: The freq is based on generic timer.
+ */
+.macro	timer_wait	reg, ticks
+	movw	\reg, #(\ticks & 0xffff)
+	movt	\reg, #(\ticks >> 16)
+	mcr	p15, 0, \reg, c14, c2, 0
+	isb
+	mov	\reg, #3
+	mcr	p15, 0, \reg, c14, c2, 1
+1 :	isb
+	mrc	p15, 0, \reg, c14, c2, 1
+	ands	\reg, \reg, #4
+	bne	1b
+	mov	\reg, #0
+	mcr	p15, 0, \reg, c14, c2, 1
+	isb
+.endm
+
+#endif /* __ASSEMBLY__ */
+
+#endif /* _GENERICTIMER_H_ */
--

index 2e6a207..2d55782 100644
@@ -12,3 +12,7 @@ obj-y	+= fsl_epu.o
 obj-$(CONFIG_OF_LIBFDT) += fdt.o
 obj-$(CONFIG_SYS_HAS_SERDES) += fsl_ls1_serdes.o ls102xa_serdes.o
 obj-$(CONFIG_SPL) += spl.o
+
+ifdef CONFIG_ARMV7_PSCI
+obj-y  += psci.o
+endif

index ca913b0..7232cd9 100644
@@ -11,6 +11,8 @@
 
 #define CONFIG_LS102XA
 
+#define CONFIG_ARMV7_PSCI
+
 #define CONFIG_SYS_GENERIC_BOARD
 
 #define CONFIG_DISPLAY_CPUINFO

index 6b6f2ba..b618be5 100644
@@ -11,6 +11,8 @@
 
 #define CONFIG_LS102XA
 
+#define CONFIG_ARMV7_PSCI
+
 #define CONFIG_SYS_GENERIC_BOARD
 
 #define CONFIG_DISPLAY_CPUINFO

2.1.0.27.g96db324
index 2e6a207..2d55782 100644
@@ -12,3 +12,7 @@ obj-y	+= fsl_epu.o
 obj-$(CONFIG_OF_LIBFDT) += fdt.o
 obj-$(CONFIG_SYS_HAS_SERDES) += fsl_ls1_serdes.o ls102xa_serdes.o
 obj-$(CONFIG_SPL) += spl.o
+
+ifdef CONFIG_ARMV7_PSCI
+obj-y  += psci.o
+endif

new file mode 100644
index 0000000..cf5cd48
@@ -0,0 +1,126 @@
+/*
+ * Copyright 2015 Freescale Semiconductor, Inc.
+ * Author: Wang Dongsheng <[email protected]>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <config.h>
+#include <linux/linkage.h>
+
+#include <asm/armv7.h>
+#include <asm/arch-armv7/generictimer.h> #include <asm/psci.h>
+
+#define SCFG_CORE0_SFT_RST      0x130
+#define SCFG_CORESRENCR         0x204
+
+#define DCFG_CCSR_BRR           0x0E4
+#define DCFG_CCSR_SCRATCHRW1    0x200
+
+	.pushsection ._secure.text, "ax"
+
+	.arch_extension sec
+
+#define	ONE_MS		(GENERIC_TIMER_CLK / 1000)
+#define	RESET_WAIT	(30 * ONE_MS)
+
+	@ r1 = target CPU
+	@ r2 = target PC
+.globl	psci_cpu_on
+psci_cpu_on:
+	push	{lr}
+
+	@ Clear and Get the correct CPU number
+	@ r1 = 0xf01
+	and	r1, r1, #0xff
+
+	mov	r0, r1
+	bl	psci_get_cpu_stack_top
+	str	r2, [r0]
+	dsb
+
+	@ Get DCFG base address
+	movw	r4, #(CONFIG_SYS_FSL_GUTS_ADDR & 0xffff)
+	movt	r4, #(CONFIG_SYS_FSL_GUTS_ADDR >> 16)
+
+	@ Detect target CPU state
+	ldr	r2, [r4, #DCFG_CCSR_BRR]
+	rev	r2, r2
+	lsr	r2, r2, r1
+	ands	r2, r2, #1
+	beq	holdoff_release
+
+	@ Reset target CPU
+	@ Get SCFG base address
+	movw	r0, #(CONFIG_SYS_FSL_SCFG_ADDR & 0xffff)
+	movt	r0, #(CONFIG_SYS_FSL_SCFG_ADDR >> 16)
+
+	@ Enable CORE Soft Reset
+	movw	r5, #0
+	movt	r5, #(1 << 15)
+	rev	r5, r5
+	str	r5, [r0, #SCFG_CORESRENCR]
+
+	@ Get CPUx offset register
+	mov	r6, #0x4
+	mul	r6, r6, r1
+	add	r2, r0, r6
+
+	@ Do reset on target CPU
+	movw	r5, #0
+	movt	r5, #(1 << 15)
+	rev	r5, r5
+	str	r5, [r2, #SCFG_CORE0_SFT_RST]
+
+	@ Wait target CPU up
+	timer_wait	r2, RESET_WAIT
+
+	@ Disable CORE soft reset
+	mov	r5, #0
+	str	r5, [r0, #SCFG_CORESRENCR]
+
+holdoff_release:
+	@ Release on target CPU
+	ldr	r2, [r4, #DCFG_CCSR_BRR]
+	mov	r6, #1
+	lsl	r6, r6, r1	@ 32 bytes per CPU
+
+	rev	r6, r6
+	orr	r2, r2, r6
+	str	r2, [r4, #DCFG_CCSR_BRR]
+
+	@ Set secondary boot entry
+	ldr	r6, =psci_cpu_entry
+	rev	r6, r6
+	str	r6, [r4, #DCFG_CCSR_SCRATCHRW1]
+
+	isb
+	dsb
+
+	@ Return
+	mov	r0, #ARM_PSCI_RET_SUCCESS
+
+	pop	{lr}
+	bx	lr
+
+.globl	psci_cpu_off
+psci_cpu_off:
+	bl	psci_cpu_off_common
+
+1:	wfi
+	b	1b
+
+.globl	psci_arch_init
+psci_arch_init:
+	mov	r6, lr
+
+	bl	psci_get_cpu_id
+	bl	psci_get_cpu_stack_top
+	mov	sp, r0
+
+	bx	r6
+
+	.globl psci_text_end
+psci_text_end:
+	.popsection

@@ -11,6 +11,8 @@
 
 #define CONFIG_LS102XA
 
+#define CONFIG_ARMV7_PSCI
+
 #define CONFIG_SYS_GENERIC_BOARD
 
 #define CONFIG_DISPLAY_CPUINFO

@@ -11,6 +11,8 @@
 
 #define CONFIG_LS102XA
 
+#define CONFIG_ARMV7_PSCI
+
 #define CONFIG_SYS_GENERIC_BOARD
 
 #define CONFIG_DISPLAY_CPUINFO
--

index 2e6a207..2d55782 100644
@@ -12,3 +12,7 @@ obj-y	+= fsl_epu.o
 obj-$(CONFIG_OF_LIBFDT) += fdt.o
 obj-$(CONFIG_SYS_HAS_SERDES) += fsl_ls1_serdes.o ls102xa_serdes.o
 obj-$(CONFIG_SPL) += spl.o
+
+ifdef CONFIG_ARMV7_PSCI
+obj-y  += psci.o
+endif

new file mode 100644
index 0000000..cf5cd48
@@ -0,0 +1,126 @@
+/*
+ * Copyright 2015 Freescale Semiconductor, Inc.
+ * Author: Wang Dongsheng <[email protected]>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <config.h>
+#include <linux/linkage.h>
+
+#include <asm/armv7.h>
+#include <asm/arch-armv7/generictimer.h> #include <asm/psci.h>
+
+#define SCFG_CORE0_SFT_RST      0x130
+#define SCFG_CORESRENCR         0x204
+
+#define DCFG_CCSR_BRR           0x0E4
+#define DCFG_CCSR_SCRATCHRW1    0x200
+
+	.pushsection ._secure.text, "ax"
+
+	.arch_extension sec
+
+#define	ONE_MS		(GENERIC_TIMER_CLK / 1000)
+#define	RESET_WAIT	(30 * ONE_MS)
+
+	@ r1 = target CPU
+	@ r2 = target PC
+.globl	psci_cpu_on
+psci_cpu_on:
+	push	{lr}
+
+	@ Clear and Get the correct CPU number
+	@ r1 = 0xf01
+	and	r1, r1, #0xff
+
+	mov	r0, r1
+	bl	psci_get_cpu_stack_top
+	str	r2, [r0]
+	dsb
+
+	@ Get DCFG base address
+	movw	r4, #(CONFIG_SYS_FSL_GUTS_ADDR & 0xffff)
+	movt	r4, #(CONFIG_SYS_FSL_GUTS_ADDR >> 16)
+
+	@ Detect target CPU state
+	ldr	r2, [r4, #DCFG_CCSR_BRR]
+	rev	r2, r2
+	lsr	r2, r2, r1
+	ands	r2, r2, #1
+	beq	holdoff_release
+
+	@ Reset target CPU
+	@ Get SCFG base address
+	movw	r0, #(CONFIG_SYS_FSL_SCFG_ADDR & 0xffff)
+	movt	r0, #(CONFIG_SYS_FSL_SCFG_ADDR >> 16)
+
+	@ Enable CORE Soft Reset
+	movw	r5, #0
+	movt	r5, #(1 << 15)
+	rev	r5, r5
+	str	r5, [r0, #SCFG_CORESRENCR]
+
+	@ Get CPUx offset register
+	mov	r6, #0x4
+	mul	r6, r6, r1
+	add	r2, r0, r6
+
+	@ Do reset on target CPU
+	movw	r5, #0
+	movt	r5, #(1 << 15)
+	rev	r5, r5
+	str	r5, [r2, #SCFG_CORE0_SFT_RST]
+
+	@ Wait target CPU up
+	timer_wait	r2, RESET_WAIT
+
+	@ Disable CORE soft reset
+	mov	r5, #0
+	str	r5, [r0, #SCFG_CORESRENCR]
+
+holdoff_release:
+	@ Release on target CPU
+	ldr	r2, [r4, #DCFG_CCSR_BRR]
+	mov	r6, #1
+	lsl	r6, r6, r1	@ 32 bytes per CPU
+
+	rev	r6, r6
+	orr	r2, r2, r6
+	str	r2, [r4, #DCFG_CCSR_BRR]
+
+	@ Set secondary boot entry
+	ldr	r6, =psci_cpu_entry
+	rev	r6, r6
+	str	r6, [r4, #DCFG_CCSR_SCRATCHRW1]
+
+	isb
+	dsb
+
+	@ Return
+	mov	r0, #ARM_PSCI_RET_SUCCESS
+
+	pop	{lr}
+	bx	lr
+
+.globl	psci_cpu_off
+psci_cpu_off:
+	bl	psci_cpu_off_common
+
+1:	wfi
+	b	1b
+
+.globl	psci_arch_init
+psci_arch_init:
+	mov	r6, lr
+
+	bl	psci_get_cpu_id
+	bl	psci_get_cpu_stack_top
+	mov	sp, r0
+
+	bx	r6
+
+	.globl psci_text_end
+psci_text_end:
+	.popsection

@@ -11,6 +11,8 @@
 
 #define CONFIG_LS102XA
 
+#define CONFIG_ARMV7_PSCI
+
 #define CONFIG_SYS_GENERIC_BOARD
 
 #define CONFIG_DISPLAY_CPUINFO

@@ -11,6 +11,8 @@
 
 #define CONFIG_LS102XA
 
+#define CONFIG_ARMV7_PSCI
+
 #define CONFIG_SYS_GENERIC_BOARD
 
 #define CONFIG_DISPLAY_CPUINFO
--

index 2e6a207..2d55782 100644
@@ -12,3 +12,7 @@ obj-y	+= fsl_epu.o
 obj-$(CONFIG_OF_LIBFDT) += fdt.o
 obj-$(CONFIG_SYS_HAS_SERDES) += fsl_ls1_serdes.o ls102xa_serdes.o
 obj-$(CONFIG_SPL) += spl.o
+
+ifdef CONFIG_ARMV7_PSCI
+obj-y  += psci.o
+endif

new file mode 100644
index 0000000..cf5cd48
@@ -0,0 +1,126 @@
+/*
+ * Copyright 2015 Freescale Semiconductor, Inc.
+ * Author: Wang Dongsheng <[email protected]>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <config.h>
+#include <linux/linkage.h>
+
+#include <asm/armv7.h>
+#include <asm/arch-armv7/generictimer.h> #include <asm/psci.h>
+
+#define SCFG_CORE0_SFT_RST      0x130
+#define SCFG_CORESRENCR         0x204
+
+#define DCFG_CCSR_BRR           0x0E4
+#define DCFG_CCSR_SCRATCHRW1    0x200
+
+	.pushsection ._secure.text, "ax"
+
+	.arch_extension sec
+
+#define	ONE_MS		(GENERIC_TIMER_CLK / 1000)
+#define	RESET_WAIT	(30 * ONE_MS)
+
+	@ r1 = target CPU
+	@ r2 = target PC
+.globl	psci_cpu_on
+psci_cpu_on:
+	push	{lr}
+
+	@ Clear and Get the correct CPU number
+	@ r1 = 0xf01
+	and	r1, r1, #0xff
+
+	mov	r0, r1
+	bl	psci_get_cpu_stack_top
+	str	r2, [r0]
+	dsb
+
+	@ Get DCFG base address
+	movw	r4, #(CONFIG_SYS_FSL_GUTS_ADDR & 0xffff)
+	movt	r4, #(CONFIG_SYS_FSL_GUTS_ADDR >> 16)
+
+	@ Detect target CPU state
+	ldr	r2, [r4, #DCFG_CCSR_BRR]
+	rev	r2, r2
+	lsr	r2, r2, r1
+	ands	r2, r2, #1
+	beq	holdoff_release
+
+	@ Reset target CPU
+	@ Get SCFG base address
+	movw	r0, #(CONFIG_SYS_FSL_SCFG_ADDR & 0xffff)
+	movt	r0, #(CONFIG_SYS_FSL_SCFG_ADDR >> 16)
+
+	@ Enable CORE Soft Reset
+	movw	r5, #0
+	movt	r5, #(1 << 15)
+	rev	r5, r5
+	str	r5, [r0, #SCFG_CORESRENCR]
+
+	@ Get CPUx offset register
+	mov	r6, #0x4
+	mul	r6, r6, r1
+	add	r2, r0, r6
+
+	@ Do reset on target CPU
+	movw	r5, #0
+	movt	r5, #(1 << 15)
+	rev	r5, r5
+	str	r5, [r2, #SCFG_CORE0_SFT_RST]
+
+	@ Wait target CPU up
+	timer_wait	r2, RESET_WAIT
+
+	@ Disable CORE soft reset
+	mov	r5, #0
+	str	r5, [r0, #SCFG_CORESRENCR]
+
+holdoff_release:
+	@ Release on target CPU
+	ldr	r2, [r4, #DCFG_CCSR_BRR]
+	mov	r6, #1
+	lsl	r6, r6, r1	@ 32 bytes per CPU
+
+	rev	r6, r6
+	orr	r2, r2, r6
+	str	r2, [r4, #DCFG_CCSR_BRR]
+
+	@ Set secondary boot entry
+	ldr	r6, =psci_cpu_entry
+	rev	r6, r6
+	str	r6, [r4, #DCFG_CCSR_SCRATCHRW1]
+
+	isb
+	dsb
+
+	@ Return
+	mov	r0, #ARM_PSCI_RET_SUCCESS
+
+	pop	{lr}
+	bx	lr
+
+.globl	psci_cpu_off
+psci_cpu_off:
+	bl	psci_cpu_off_common
+
+1:	wfi
+	b	1b
+
+.globl	psci_arch_init
+psci_arch_init:
+	mov	r6, lr
+
+	bl	psci_get_cpu_id
+	bl	psci_get_cpu_stack_top
+	mov	sp, r0
+
+	bx	r6
+
+	.globl psci_text_end
+psci_text_end:
+	.popsection

@@ -11,6 +11,8 @@
 
 #define CONFIG_LS102XA
 
+#define CONFIG_ARMV7_PSCI
+
 #define CONFIG_SYS_GENERIC_BOARD
 
 #define CONFIG_DISPLAY_CPUINFO

@@ -11,6 +11,8 @@
 
 #define CONFIG_LS102XA
 

+#define CONFIG_ARMV7_PSCI
+
 #define CONFIG_SYS_GENERIC_BOARD
 
 #define CONFIG_DISPLAY_CPUINFO
--

index 2e6a207..2d55782 100644
@@ -12,3 +12,7 @@ obj-y	+= fsl_epu.o
 obj-$(CONFIG_OF_LIBFDT) += fdt.o
 obj-$(CONFIG_SYS_HAS_SERDES) += fsl_ls1_serdes.o

 obj-$(CONFIG_SPL) += spl.o
+
+ifdef CONFIG_ARMV7_PSCI
+obj-y  += psci.o
+endif

new file mode 100644
index 0000000..cf5cd48
@@ -0,0 +1,126 @@
+/*
+ * Copyright 2015 Freescale Semiconductor, Inc.
+ * Author: Wang Dongsheng <[email protected]>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <config.h>
+#include <linux/linkage.h>
+
+#include <asm/armv7.h>
+#include <asm/arch-armv7/generictimer.h> #include <asm/psci.h>
+
+#define SCFG_CORE0_SFT_RST      0x130
+#define SCFG_CORESRENCR         0x204
+
+#define DCFG_CCSR_BRR           0x0E4
+#define DCFG_CCSR_SCRATCHRW1    0x200
+
+	.pushsection ._secure.text, "ax"
+
+	.arch_extension sec
+
+#define	ONE_MS		(GENERIC_TIMER_CLK / 1000)
+#define	RESET_WAIT	(30 * ONE_MS)
+
+	@ r1 = target CPU
+	@ r2 = target PC
+.globl	psci_cpu_on
+psci_cpu_on:
+	push	{lr}
+
+	@ Clear and Get the correct CPU number
+	@ r1 = 0xf01
+	and	r1, r1, #0xff
+
+	mov	r0, r1
+	bl	psci_get_cpu_stack_top
+	str	r2, [r0]
+	dsb
+
+	@ Get DCFG base address
+	movw	r4, #(CONFIG_SYS_FSL_GUTS_ADDR & 0xffff)
+	movt	r4, #(CONFIG_SYS_FSL_GUTS_ADDR >> 16)
+
+	@ Detect target CPU state
+	ldr	r2, [r4, #DCFG_CCSR_BRR]
+	rev	r2, r2
+	lsr	r2, r2, r1
+	ands	r2, r2, #1
+	beq	holdoff_release
+
+	@ Reset target CPU
+	@ Get SCFG base address
+	movw	r0, #(CONFIG_SYS_FSL_SCFG_ADDR & 0xffff)
+	movt	r0, #(CONFIG_SYS_FSL_SCFG_ADDR >> 16)
+
+	@ Enable CORE Soft Reset
+	movw	r5, #0
+	movt	r5, #(1 << 15)
+	rev	r5, r5
+	str	r5, [r0, #SCFG_CORESRENCR]
+
+	@ Get CPUx offset register
+	mov	r6, #0x4
+	mul	r6, r6, r1
+	add	r2, r0, r6
+
+	@ Do reset on target CPU
+	movw	r5, #0
+	movt	r5, #(1 << 15)
+	rev	r5, r5
+	str	r5, [r2, #SCFG_CORE0_SFT_RST]
+
+	@ Wait target CPU up
+	timer_wait	r2, RESET_WAIT
+
+	@ Disable CORE soft reset
+	mov	r5, #0
+	str	r5, [r0, #SCFG_CORESRENCR]
+
+holdoff_release:
+	@ Release on target CPU
+	ldr	r2, [r4, #DCFG_CCSR_BRR]
+	mov	r6, #1
+	lsl	r6, r6, r1	@ 32 bytes per CPU
+
+	rev	r6, r6
+	orr	r2, r2, r6
+	str	r2, [r4, #DCFG_CCSR_BRR]
+
+	@ Set secondary boot entry
+	ldr	r6, =psci_cpu_entry
+	rev	r6, r6
+	str	r6, [r4, #DCFG_CCSR_SCRATCHRW1]
+
+	isb
+	dsb
+
+	@ Return
+	mov	r0, #ARM_PSCI_RET_SUCCESS
+
+	pop	{lr}
+	bx	lr
+
+.globl	psci_cpu_off
+psci_cpu_off:
+	bl	psci_cpu_off_common
+
+1:	wfi
+	b	1b
+
+.globl	psci_arch_init
+psci_arch_init:
+	mov	r6, lr
+
+	bl	psci_get_cpu_id
+	bl	psci_get_cpu_stack_top
+	mov	sp, r0
+
+	bx	r6
+
+	.globl psci_text_end
+psci_text_end:
+	.popsection

@@ -11,6 +11,8 @@
 
 #define CONFIG_LS102XA
 
+#define CONFIG_ARMV7_PSCI
+
 #define CONFIG_SYS_GENERIC_BOARD
 
 #define CONFIG_DISPLAY_CPUINFO

@@ -11,6 +11,8 @@
 
 #define CONFIG_LS102XA
 

+#define CONFIG_ARMV7_PSCI
+
 #define CONFIG_SYS_GENERIC_BOARD
 
 #define CONFIG_DISPLAY_CPUINFO
--

index d4cb51e..4ff46e4 100644
@@ -18,6 +18,8 @@
   */
 
  #include <config.h>
+
+#include <asm/arch-armv7/generictimer.h>
  #include <asm/gic.h>
  #include <asm/macro.h>
  #include <asm/psci.h>

@@ -43,26 +45,6 @@
  #define	GICD_BASE		0x1c81000
  #define	GICC_BASE		0x1c82000
 
-.macro	timer_wait	reg, ticks
-	@ Program CNTP_TVAL
-	movw	\reg, #(\ticks & 0xffff)
-	movt	\reg, #(\ticks >> 16)
-	mcr	p15, 0, \reg, c14, c2, 0
-	isb
-	@ Enable physical timer, mask interrupt
-	mov	\reg, #3
-	mcr	p15, 0, \reg, c14, c2, 1
-	@ Poll physical timer until ISTATUS is on
-1:	isb
-	mrc	p15, 0, \reg, c14, c2, 1
-	ands	\reg, \reg, #4
-	bne	1b
-	@ Disable timer
-	mov	\reg, #0
-	mcr	p15, 0, \reg, c14, c2, 1
-	isb
-.endm
-
  .globl	psci_fiq_enter
  psci_fiq_enter:
  	push	{r0-r12}

index bbfeec8..e15d587 100644
@@ -18,6 +18,8 @@
   */
 
  #include <config.h>
+
+#include <asm/arch-armv7/generictimer.h>
  #include <asm/gic.h>
  #include <asm/macro.h>
  #include <asm/psci.h>

@@ -43,26 +45,6 @@
  #define	GICD_BASE		0x1c81000
  #define	GICC_BASE		0x1c82000
 
-.macro	timer_wait	reg, ticks
-	@ Program CNTP_TVAL
-	movw	\reg, #(\ticks & 0xffff)
-	movt	\reg, #(\ticks >> 16)
-	mcr	p15, 0, \reg, c14, c2, 0
-	isb
-	@ Enable physical timer, mask interrupt
-	mov	\reg, #3
-	mcr	p15, 0, \reg, c14, c2, 1
-	@ Poll physical timer until ISTATUS is on
-1:	isb
-	mrc	p15, 0, \reg, c14, c2, 1
-	ands	\reg, \reg, #4
-	bne	1b
-	@ Disable timer
-	mov	\reg, #0
-	mcr	p15, 0, \reg, c14, c2, 1
-	isb
-.endm
-
  .globl	psci_fiq_enter
  psci_fiq_enter:
  	push	{r0-r12}

new file mode 100644
index 0000000..0956d7c
@@ -0,0 +1,50 @@
+/*
+ * Copyright (C) 2013 - ARM Ltd
+ * Author: Marc Zyngier <[email protected]>
+ *
+ * Based on code by Carl van Schaik <[email protected]>.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef _GENERICTIMER_H_
+#define _GENERICTIMER_H_
+
+#ifdef __ASSEMBLY__
+
+/*
+ * This macro provide a physical timer that can be used for delay in the code.
+ * The macro is moved from sunxi/psci_sun7i.S
+ *
+ * reg: is used in this macro.
+ * ticks: The freq is based on generic timer.
+ */
+.macro	timer_wait	reg, ticks
+	movw	\reg, #(\ticks & 0xffff)
+	movt	\reg, #(\ticks >> 16)
+	mcr	p15, 0, \reg, c14, c2, 0
+	isb
+	mov	\reg, #3
+	mcr	p15, 0, \reg, c14, c2, 1
+1 :	isb
+	mrc	p15, 0, \reg, c14, c2, 1
+	ands	\reg, \reg, #4
+	bne	1b
+	mov	\reg, #0
+	mcr	p15, 0, \reg, c14, c2, 1
+	isb
+.endm
+
+#endif /* __ASSEMBLY__ */
+
+#endif /* _GENERICTIMER_H_ */


ARMv7: Factor out reusable timer_wait from sunxi/psci_sun7i.S

Dongsheng Wang

Wang Dongsheng

Dongsheng Wang

Wang Dongsheng

Wang Dongsheng

Wang Dongsheng

Huan Wang

York Sun

Hans de Goede

Wang Dongsheng

Hans de Goede

Wang Dongsheng

York Sun

Hans de Goede

Wang Dongsheng

York Sun

Wang Dongsheng