1ddfb28f70d955d80c63443456f067dbc42faf39
[linux-2.6.git] / arch / arm / mach-tegra / headsmp.S
1 /*
2  * arch/arm/mach-tegra/headsmp.S
3  *
4  * SMP initialization routines for Tegra SoCs
5  *
6  * Copyright (c) 2009-2010, NVIDIA Corporation.
7  * Copyright (c) 2011 Google, Inc.
8  * Author: Colin Cross <ccross@android.com>
9  *         Gary King <gking@nvidia.com>
10  *
11  * This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful, but WITHOUT
17  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
18  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
19  * more details.
20  */
21
22 #include <linux/linkage.h>
23 #include <linux/init.h>
24
25 #include <asm/assembler.h>
26 #include <asm/cache.h>
27
28 #include <mach/iomap.h>
29
30 #ifdef CONFIG_SMP
31 /*
32  *      tegra_secondary_startup
33  *
34  *       Initial secondary processor boot vector; jumps to kernel's
35  *       secondary_startup routine
36  */
37 #ifdef CONFIG_SMP
38 ENTRY(tegra_secondary_startup)
39         bl      tegra_invalidate_l1
40         bl      tegra_enable_coresite
41         b       secondary_startup
42 ENDPROC(tegra_secondary_startup)
43 #endif
44
45 #ifdef CONFIG_PM
46 /*
47  *      tegra_secondary_resume
48  *
49  *        Secondary CPU boot vector when restarting a CPU following lp2 idle.
50  */
51 ENTRY(tegra_secondary_resume)
52         bl      tegra_invalidate_l1
53         bl      tegra_enable_coresite
54         b       cpu_resume
55 ENDPROC(tegra_secondary_resume)
56 #endif
57 #endif
58
59 #ifdef CONFIG_PM
60 /*
61  *      tegra_resume
62  *
63  *        CPU boot vector when restarting the master CPU following
64  *        an LP2 transition. Also branched to by LP0 and LP1 resume after
65  *        re-enabling sdram.
66  */
67 ENTRY(tegra_resume)
68         bl      tegra_invalidate_l1
69         bl      tegra_enable_coresite
70
71         /* enable SCU */
72         ldr     r0, =TEGRA_ARM_PERIF_BASE
73         ldr     r1, [r0]
74         orr     r1, r1, #1
75         str     r1, [r0]
76
77         b       cpu_resume
78 ENDPROC(tegra_resume)
79 #endif
80
81 /*
82  *      tegra_invalidate_l1
83  *
84  *        Invalidates the L1 data cache (no clean) during initial boot of a cpu
85  *
86  *        Corrupted registers: r0-r6
87  */
88 tegra_invalidate_l1:
89         mov     r0, #0
90         mcr     p15, 2, r0, c0, c0, 0
91         mrc     p15, 1, r0, c0, c0, 0
92
93         movw    r1, #0x7fff
94         and     r2, r1, r0, lsr #13
95
96         movw    r1, #0x3ff
97
98         and     r3, r1, r0, lsr #3      @ NumWays - 1
99         add     r2, r2, #1      @ NumSets
100
101         and     r0, r0, #0x7
102         add     r0, r0, #4      @ SetShift
103
104         clz     r1, r3          @ WayShift
105         add     r4, r3, #1      @ NumWays
106 1:      sub     r2, r2, #1      @ NumSets--
107         mov     r3, r4          @ Temp = NumWays
108 2:      subs    r3, r3, #1      @ Temp--
109         mov     r5, r3, lsl r1
110         mov     r6, r2, lsl r0
111         orr     r5, r5, r6      @ Reg = (Temp<<WayShift)|(NumSets<<SetShift)
112         mcr     p15, 0, r5, c7, c6, 2
113         bgt     2b
114         cmp     r2, #0
115         bgt     1b
116         dsb
117         isb
118         mov     pc, lr
119
120         /* Enable Coresight access on cpu */
121 tegra_enable_coresite:
122         ldr     r0, =0xC5ACCE55
123         mcr     p14, 0, r0, c7, c12, 6
124         mov     pc, lr