summaryrefslogtreecommitdiffstats
path: root/arch/arm/mach-shmobile/headsmp.S
blob: cef8e8c555f84b926adfa95e45c421b6759c665a (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
/*
 * SMP support for R-Mobile / SH-Mobile
 *
 * Copyright (C) 2010  Magnus Damm
 * Copyright (C) 2010  Takashi Yoshii
 *
 * Based on vexpress, Copyright (c) 2003 ARM Limited, All Rights Reserved
 *
 * 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.
 */
#include <linux/init.h>
#include <linux/linkage.h>
#include <linux/threads.h>
#include <asm/assembler.h>
#include <asm/memory.h>

#define SCTLR_MMU	0x01
#define BOOTROM_ADDRESS	0xE6340000
#define RWTCSRA_ADDRESS 0xE6020004
#define RWTCSRA_WOVF	0x10

/*
 * Reset vector for secondary CPUs.
 * This will be mapped at address 0 by SBAR register.
 * We need _long_ jump to the physical address.
 */
	.arm
	.align  12
ENTRY(shmobile_boot_vector)
	ldr     r1, 1f
	bx	r1

ENDPROC(shmobile_boot_vector)

	.align	2
	.globl	shmobile_boot_fn
shmobile_boot_fn:
1:	.space	4
	.globl	shmobile_boot_size
shmobile_boot_size:
	.long	. - shmobile_boot_vector

#ifdef CONFIG_ARCH_RCAR_GEN2
/*
 * Reset vector for R-Car Gen2 and RZ/G1 secondary CPUs.
 * This will be mapped at address 0 by SBAR register.
 */
ENTRY(shmobile_boot_vector_gen2)
	mrc	p15, 0, r0, c0, c0, 5		@ r0 = MPIDR
	ldr	r1, shmobile_boot_cpu_gen2
	cmp	r0, r1
	bne	shmobile_smp_continue_gen2

	mrc	p15, 0, r1, c1, c0, 0		@ r1 = SCTLR
	and	r0, r1, #SCTLR_MMU
	cmp	r0, #SCTLR_MMU
	beq	shmobile_smp_continue_gen2

	ldr	r0, rwtcsra
	mov	r1, #0
	ldrb	r1, [r0]
	and	r0, r1, #RWTCSRA_WOVF
	cmp	r0, #RWTCSRA_WOVF
	bne	shmobile_smp_continue_gen2

	ldr	r0, bootrom
	bx	r0

shmobile_smp_continue_gen2:
	ldr     r1, shmobile_boot_fn_gen2
	bx	r1

ENDPROC(shmobile_boot_vector_gen2)

	.align	4
rwtcsra:
	.word	RWTCSRA_ADDRESS
bootrom:
	.word	BOOTROM_ADDRESS
	.globl	shmobile_boot_cpu_gen2
shmobile_boot_cpu_gen2:
	.word	0x00000000

	.align	2
	.globl	shmobile_boot_fn_gen2
shmobile_boot_fn_gen2:
	.space	4
	.globl	shmobile_boot_size_gen2
shmobile_boot_size_gen2:
	.long	. - shmobile_boot_vector_gen2
#endif /* CONFIG_ARCH_RCAR_GEN2 */

/*
 * Per-CPU SMP boot function/argument selection code based on MPIDR
 */

ENTRY(shmobile_smp_boot)
	mrc	p15, 0, r1, c0, c0, 5		@ r1 = MPIDR
	and	r0, r1, #0xffffff		@ MPIDR_HWID_BITMASK
						@ r0 = cpu_logical_map() value
	mov	r1, #0				@ r1 = CPU index
	adr	r2, 1f
	ldmia	r2, {r5, r6, r7}
	add	r5, r5, r2			@ array of per-cpu mpidr values
	add	r6, r6, r2			@ array of per-cpu functions
	add	r7, r7, r2			@ array of per-cpu arguments

shmobile_smp_boot_find_mpidr:
	ldr	r8, [r5, r1, lsl #2]
	cmp	r8, r0
	bne	shmobile_smp_boot_next

	ldr	r9, [r6, r1, lsl #2]
	cmp	r9, #0
	bne	shmobile_smp_boot_found

shmobile_smp_boot_next:
	add	r1, r1, #1
	cmp	r1, #NR_CPUS
	blo	shmobile_smp_boot_find_mpidr

	b	shmobile_smp_sleep

shmobile_smp_boot_found:
	ldr	r0, [r7, r1, lsl #2]
	ret	r9
ENDPROC(shmobile_smp_boot)

ENTRY(shmobile_smp_sleep)
	wfi
	b	shmobile_smp_boot
ENDPROC(shmobile_smp_sleep)

	.align	2
1:	.long	shmobile_smp_mpidr - .
	.long	shmobile_smp_fn - 1b
	.long	shmobile_smp_arg - 1b

	.bss
	.globl	shmobile_smp_mpidr
shmobile_smp_mpidr:
	.space	NR_CPUS * 4
	.globl	shmobile_smp_fn
shmobile_smp_fn:
	.space	NR_CPUS * 4
	.globl	shmobile_smp_arg
shmobile_smp_arg:
	.space	NR_CPUS * 4