1/* SPDX-License-Identifier: GPL-2.0+ */
2/*
3 * Copyright 2018 NXP
4 */
5
6#include <config.h>
7#include <linux/linkage.h>
8
9#include <asm/armv7.h>
10#include <asm/psci.h>
11
12	.pushsection ._secure.text, "ax"
13
14	.arch_extension sec
15
16.globl v7_invalidate_l1
17v7_invalidate_l1:
18	mov	r0, #0
19	mcr	p15, 2, r0, c0, c0, 0
20	mrc	p15, 1, r0, c0, c0, 0
21
22	movw	r1, #0x7fff
23	and	r2, r1, r0, lsr #13
24
25	movw	r1, #0x3ff
26
27	and	r3, r1, r0, lsr #3      @ NumWays - 1
28	add	r2, r2, #1              @ NumSets
29
30	and	r0, r0, #0x7
31	add	r0, r0, #4      @ SetShift
32
33	clz	r1, r3          @ WayShift
34	add	r4, r3, #1      @ NumWays
351:
36	sub	r2, r2, #1      @ NumSets--
37	mov	r3, r4          @ Temp = NumWays
382:
39	subs	r3, r3, #1      @ Temp--
40	mov	r5, r3, lsl r1
41	mov	r6, r2, lsl r0
42	orr	r5, r5, r6      @ Reg = (Temp<<WayShift)|(NumSets<<SetShift)
43	mcr	p15, 0, r5, c7, c6, 2
44	bgt	2b
45	cmp	r2, #0
46	bgt	1b
47	dsb	st
48	isb
49	mov	pc, lr
50
51.globl psci_system_resume
52psci_system_resume:
53	mov	sp, r0
54
55	/* invalidate L1 I-cache first */
56	mov	r6, #0x0
57	mcr	p15, 0, r6, c7, c5, 0
58	mcr	p15, 0, r6, c7, c5, 6
59	/* enable the Icache and branch prediction */
60	mov	r6, #0x1800
61	mcr	p15, 0, r6, c1, c0, 0
62	isb
63
64	bl	v7_invalidate_l1
65	b	imx_system_resume
66
67	.popsection
68