From patchwork Wed Dec 31 12:46:40 2014 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: Jan Kiszka X-Patchwork-Id: 424778 X-Patchwork-Delegate: ijc@hellion.org.uk Return-Path: X-Original-To: incoming@patchwork.ozlabs.org Delivered-To: patchwork-incoming@bilbo.ozlabs.org Received: from theia.denx.de (theia.denx.de [85.214.87.163]) by ozlabs.org (Postfix) with ESMTP id 8E0451400EA for ; Wed, 31 Dec 2014 23:47:04 +1100 (AEDT) Received: from localhost (localhost [127.0.0.1]) by theia.denx.de (Postfix) with ESMTP id 6E1B34B61C; Wed, 31 Dec 2014 13:47:02 +0100 (CET) X-Amavis-Alert: BAD HEADER SECTION, Duplicate header field: "References" Received: from theia.denx.de ([127.0.0.1]) by localhost (theia.denx.de [127.0.0.1]) (amavisd-new, port 10024) with ESMTP id zpGgi4Bq8t9o; Wed, 31 Dec 2014 13:47:01 +0100 (CET) Received: from theia.denx.de (localhost [127.0.0.1]) by theia.denx.de (Postfix) with ESMTP id 89D774B615; Wed, 31 Dec 2014 13:47:01 +0100 (CET) Received: from localhost (localhost [127.0.0.1]) by theia.denx.de (Postfix) with ESMTP id 6CE444B615 for ; Wed, 31 Dec 2014 13:46:57 +0100 (CET) X-Amavis-Alert: BAD HEADER SECTION, Duplicate header field: "References" Received: from theia.denx.de ([127.0.0.1]) by localhost (theia.denx.de [127.0.0.1]) (amavisd-new, port 10024) with ESMTP id APIE1-Bj0ySW for ; Wed, 31 Dec 2014 13:46:57 +0100 (CET) X-policyd-weight: NOT_IN_SBL_XBL_SPAMHAUS=-1.5 NOT_IN_SPAMCOP=-1.5 NOT_IN_BL_NJABL=-1.5 (only DNSBL check requested) Received: from mout.web.de (mout.web.de [212.227.17.11]) by theia.denx.de (Postfix) with ESMTPS id 470D74B610 for ; Wed, 31 Dec 2014 13:46:53 +0100 (CET) Received: from md1f2u6c.ww002.siemens.net ([95.157.58.223]) by smtp.web.de (mrweb103) with ESMTPSA (Nemesis) id 0MBTIY-1Xy1Sr2SFa-00AYXf; Wed, 31 Dec 2014 13:46:47 +0100 From: Jan Kiszka To: U-Boot Mailing List Date: Wed, 31 Dec 2014 13:46:40 +0100 Message-Id: <7d1e08bb90d41aa04c5948133867e43e6d0e4217.1420030003.git.jan.kiszka@web.de> X-Mailer: git-send-email 1.8.4.5 In-Reply-To: References: In-Reply-To: References: X-Provags-ID: V03:K0:eJhzr1H6smaAyBtiJLb7NqjyZIj0qBK+BESUyIi7wugtxo/6fsz YZR+7Ep299Wi2A81NobI8xElbe6afW5yGT88u4E+Iw2jIT60caLndq7qWScALX7rv4QQaNs 2XYAqNzbQS+SPueOreveZFI9rfSMgCNfr1sUD7oLTE5hdPY42C+OI/0/IhOFzO5OBet3Yb+ VHvrmfSv97vXTUThmeBlQ== X-UI-Out-Filterresults: notjunk:1; Cc: Marc Zyngier , Ian Campbell Subject: [U-Boot] [PATCH v3 1/4] sun7i: Add support for taking CPUs offline via PSCI X-BeenThere: u-boot@lists.denx.de X-Mailman-Version: 2.1.13 Precedence: list List-Id: U-Boot discussion List-Unsubscribe: , List-Archive: List-Post: List-Help: List-Subscribe: , MIME-Version: 1.0 Sender: u-boot-bounces@lists.denx.de Errors-To: u-boot-bounces@lists.denx.de From: Jan Kiszka Based on the original version by Marc Zyngier. It adds a psci_cpu_off implementation for the A20 SoC. The mechanism works by first preparing the calling CPU to go offline (disable and flush cache, disable SMP), then requesting CPU 0 to pull the plug. The request is sent as FIQ on SGI15. Signed-off-by: Jan Kiszka Acked-by: Ian Campbell --- arch/arm/cpu/armv7/sunxi/psci.S | 171 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 171 insertions(+) diff --git a/arch/arm/cpu/armv7/sunxi/psci.S b/arch/arm/cpu/armv7/sunxi/psci.S index a84807d..739ce77 100644 --- a/arch/arm/cpu/armv7/sunxi/psci.S +++ b/arch/arm/cpu/armv7/sunxi/psci.S @@ -18,6 +18,7 @@ */ #include +#include #include #include @@ -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,78 @@ 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 + + @ Validate reason based on IAR and acknowledge + movw r8, #(GICC_BASE & 0xffff) + movt r8, #(GICC_BASE >> 16) + ldr r9, [r8, #GICC_IAR] + movw r10, #0x3ff + movt r10, #0 + cmp r9, r10 @ skip spurious interrupt 1023 + beq out + movw r10, #0x3fe @ ...and 1022 + cmp r9, r10 + beq out + str r9, [r8, #GICC_EOIR] @ acknowledge the interrupt + 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 +239,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 at 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 +300,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