1/* SPDX-License-Identifier: GPL-2.0-only */
2/*
3 *  Copyright (C) 2015 Russell King
4 *
5 * This assembly is required to safely remap the physical address space
6 * for Keystone 2
7 */
8#include <linux/linkage.h>
9#include <linux/pgtable.h>
10#include <asm/asm-offsets.h>
11#include <asm/cp15.h>
12#include <asm/page.h>
13
14	.section ".idmap.text", "ax"
15
16#define L1_ORDER 3
17#define L2_ORDER 3
18
19ENTRY(lpae_pgtables_remap_asm)
20	stmfd	sp!, {r4-r8, lr}
21
22	mrc	p15, 0, r8, c1, c0, 0		@ read control reg
23	bic	ip, r8, #CR_M			@ disable caches and MMU
24	mcr	p15, 0, ip, c1, c0, 0
25	dsb
26	isb
27
28	/* Update level 2 entries covering the kernel */
29	ldr	r6, =(_end - 1)
30	add	r7, r2, #0x1000
31	add	r6, r7, r6, lsr #SECTION_SHIFT - L2_ORDER
32	add	r7, r7, #KERNEL_OFFSET >> (SECTION_SHIFT - L2_ORDER)
331:	ldrd	r4, r5, [r7]
34	adds	r4, r4, r0
35	adc	r5, r5, r1
36	strd	r4, r5, [r7], #1 << L2_ORDER
37	cmp	r7, r6
38	bls	1b
39
40	/* Update level 2 entries for the boot data */
41	add	r7, r2, #0x1000
42	movw	r3, #FDT_FIXED_BASE >> (SECTION_SHIFT - L2_ORDER)
43	add	r7, r7, r3
44	ldrd	r4, r5, [r7]
45	adds	r4, r4, r0
46	adc	r5, r5, r1
47	strd	r4, r5, [r7], #1 << L2_ORDER
48	ldrd	r4, r5, [r7]
49	adds	r4, r4, r0
50	adc	r5, r5, r1
51	strd	r4, r5, [r7]
52
53	/* Update level 1 entries */
54	mov	r6, #4
55	mov	r7, r2
562:	ldrd	r4, r5, [r7]
57	adds	r4, r4, r0
58	adc	r5, r5, r1
59	strd	r4, r5, [r7], #1 << L1_ORDER
60	subs	r6, r6, #1
61	bne	2b
62
63	mrrc	p15, 0, r4, r5, c2		@ read TTBR0
64	adds	r4, r4, r0			@ update physical address
65	adc	r5, r5, r1
66	mcrr	p15, 0, r4, r5, c2		@ write back TTBR0
67	mrrc	p15, 1, r4, r5, c2		@ read TTBR1
68	adds	r4, r4, r0			@ update physical address
69	adc	r5, r5, r1
70	mcrr	p15, 1, r4, r5, c2		@ write back TTBR1
71
72	dsb
73
74	mov	ip, #0
75	mcr	p15, 0, ip, c7, c5, 0		@ I+BTB cache invalidate
76	mcr	p15, 0, ip, c8, c7, 0		@ local_flush_tlb_all()
77	dsb
78	isb
79
80	mcr	p15, 0, r8, c1, c0, 0		@ re-enable MMU
81	dsb
82	isb
83
84	ldmfd	sp!, {r4-r8, pc}
85ENDPROC(lpae_pgtables_remap_asm)
86