diff mbox series

[v2,05/21] arm: socfpga: Override 'lowlevel_init' to support ATF

Message ID 1582115146-28658-6-git-send-email-chee.hong.ang@intel.com
State Superseded
Headers show
Series Enable ARM Trusted Firmware for U-Boot | expand

Commit Message

Ang, Chee Hong Feb. 19, 2020, 12:25 p.m. UTC
From: Chee Hong Ang <chee.hong.ang at intel.com>

Override 'lowlevel_init' to support booting ATF from SPL
on Intel SOCFPGA (64bits) platforms.

Signed-off-by: Chee Hong Ang <chee.hong.ang at intel.com>
---
 arch/arm/mach-socfpga/Makefile        |  2 +
 arch/arm/mach-socfpga/lowlevel_init.S | 85 +++++++++++++++++++++++++++++++++++
 2 files changed, 87 insertions(+)
 create mode 100644 arch/arm/mach-socfpga/lowlevel_init.S

Comments

Marek Vasut Feb. 19, 2020, 5:13 p.m. UTC | #1
On 2/19/20 1:25 PM, chee.hong.ang at intel.com wrote:
[...]
> diff --git a/arch/arm/mach-socfpga/lowlevel_init.S b/arch/arm/mach-socfpga/lowlevel_init.S
> new file mode 100644
> index 0000000..68053a0
> --- /dev/null
> +++ b/arch/arm/mach-socfpga/lowlevel_init.S

This should be some lowlevel_init_64.S to make it clear it's only for
arm64 platforms.

> @@ -0,0 +1,85 @@
> +/* SPDX-License-Identifier: GPL-2.0 */
> +/*
> + * Copyright (C) 2019, Intel Corporation
> + */
> +
> +#include <asm-offsets.h>
> +#include <config.h>
> +#include <linux/linkage.h>
> +#include <asm/macro.h>
> +
> +ENTRY(lowlevel_init)
> +	mov	x29, lr			/* Save LR */
> +
> +#if defined(CONFIG_GICV2) || defined(CONFIG_GICV3)
> +#ifdef CONFIG_SPL_ATF
> +	branch_if_slave x0, 2f
> +#else
> +	branch_if_slave x0, 1f
> +#endif
> +
> +	ldr	x0, =GICD_BASE
> +	bl	gic_init_secure
> +#ifdef CONFIG_SPL_BUILD
> +	b	2f
> +#else
> +	b	3f
> +#endif

Can't this be done in C code ? Can we reduce the ifdeffery ?
Ang, Chee Hong Feb. 20, 2020, 2:27 a.m. UTC | #2
> On 2/19/20 1:25 PM, chee.hong.ang at intel.com wrote:
> [...]
> > diff --git a/arch/arm/mach-socfpga/lowlevel_init.S
> > b/arch/arm/mach-socfpga/lowlevel_init.S
> > new file mode 100644
> > index 0000000..68053a0
> > --- /dev/null
> > +++ b/arch/arm/mach-socfpga/lowlevel_init.S
> 
> This should be some lowlevel_init_64.S to make it clear it's only for
> arm64 platforms.
OK. It makes sense. Thanks.
> 
> > @@ -0,0 +1,85 @@
> > +/* SPDX-License-Identifier: GPL-2.0 */
> > +/*
> > + * Copyright (C) 2019, Intel Corporation  */
> > +
> > +#include <asm-offsets.h>
> > +#include <config.h>
> > +#include <linux/linkage.h>
> > +#include <asm/macro.h>
> > +
> > +ENTRY(lowlevel_init)
> > +	mov	x29, lr			/* Save LR */
> > +
> > +#if defined(CONFIG_GICV2) || defined(CONFIG_GICV3) #ifdef
> > +CONFIG_SPL_ATF
> > +	branch_if_slave x0, 2f
> > +#else
> > +	branch_if_slave x0, 1f
> > +#endif
> > +
> > +	ldr	x0, =GICD_BASE
> > +	bl	gic_init_secure
> > +#ifdef CONFIG_SPL_BUILD
> > +	b	2f
> > +#else
> > +	b	3f
> > +#endif
> 
> Can't this be done in C code ? Can we reduce the ifdeffery ?
This lowlevel_init function is shared by SPL and U-Boot and they
run in slightly different flow.
I don't think this can be done in C code but let me see what I can
do to further optimize the flow to reduce the ifdeffery.
Marek Vasut Feb. 20, 2020, 4:44 p.m. UTC | #3
On 2/20/20 3:27 AM, Ang, Chee Hong wrote:
>> On 2/19/20 1:25 PM, chee.hong.ang at intel.com wrote:
>> [...]
>>> diff --git a/arch/arm/mach-socfpga/lowlevel_init.S
>>> b/arch/arm/mach-socfpga/lowlevel_init.S
>>> new file mode 100644
>>> index 0000000..68053a0
>>> --- /dev/null
>>> +++ b/arch/arm/mach-socfpga/lowlevel_init.S
>>
>> This should be some lowlevel_init_64.S to make it clear it's only for
>> arm64 platforms.
> OK. It makes sense. Thanks.
>>
>>> @@ -0,0 +1,85 @@
>>> +/* SPDX-License-Identifier: GPL-2.0 */
>>> +/*
>>> + * Copyright (C) 2019, Intel Corporation  */
>>> +
>>> +#include <asm-offsets.h>
>>> +#include <config.h>
>>> +#include <linux/linkage.h>
>>> +#include <asm/macro.h>
>>> +
>>> +ENTRY(lowlevel_init)
>>> +	mov	x29, lr			/* Save LR */
>>> +
>>> +#if defined(CONFIG_GICV2) || defined(CONFIG_GICV3) #ifdef
>>> +CONFIG_SPL_ATF
>>> +	branch_if_slave x0, 2f
>>> +#else
>>> +	branch_if_slave x0, 1f
>>> +#endif
>>> +
>>> +	ldr	x0, =GICD_BASE
>>> +	bl	gic_init_secure
>>> +#ifdef CONFIG_SPL_BUILD
>>> +	b	2f
>>> +#else
>>> +	b	3f
>>> +#endif
>>
>> Can't this be done in C code ? Can we reduce the ifdeffery ?
> This lowlevel_init function is shared by SPL and U-Boot and they
> run in slightly different flow.

What does this 'different flow' mean ?

> I don't think this can be done in C code but let me see what I can
> do to further optimize the flow to reduce the ifdeffery.

That would be nice, thanks.
Ang, Chee Hong Feb. 20, 2020, 7:05 p.m. UTC | #4
> On 2/20/20 3:27 AM, Ang, Chee Hong wrote:
> >> On 2/19/20 1:25 PM, chee.hong.ang at intel.com wrote:
> >> [...]
> >>> diff --git a/arch/arm/mach-socfpga/lowlevel_init.S
> >>> b/arch/arm/mach-socfpga/lowlevel_init.S
> >>> new file mode 100644
> >>> index 0000000..68053a0
> >>> --- /dev/null
> >>> +++ b/arch/arm/mach-socfpga/lowlevel_init.S
> >>
> >> This should be some lowlevel_init_64.S to make it clear it's only for
> >> arm64 platforms.
> > OK. It makes sense. Thanks.
> >>
> >>> @@ -0,0 +1,85 @@
> >>> +/* SPDX-License-Identifier: GPL-2.0 */
> >>> +/*
> >>> + * Copyright (C) 2019, Intel Corporation  */
> >>> +
> >>> +#include <asm-offsets.h>
> >>> +#include <config.h>
> >>> +#include <linux/linkage.h>
> >>> +#include <asm/macro.h>
> >>> +
> >>> +ENTRY(lowlevel_init)
> >>> +	mov	x29, lr			/* Save LR */
> >>> +
> >>> +#if defined(CONFIG_GICV2) || defined(CONFIG_GICV3) #ifdef
> >>> +CONFIG_SPL_ATF
> >>> +	branch_if_slave x0, 2f
> >>> +#else
> >>> +	branch_if_slave x0, 1f
> >>> +#endif
> >>> +
> >>> +	ldr	x0, =GICD_BASE
> >>> +	bl	gic_init_secure
> >>> +#ifdef CONFIG_SPL_BUILD
> >>> +	b	2f
> >>> +#else
> >>> +	b	3f
> >>> +#endif
> >>
> >> Can't this be done in C code ? Can we reduce the ifdeffery ?
> > This lowlevel_init function is shared by SPL and U-Boot and they run
> > in slightly different flow.
> 
> What does this 'different flow' mean ?
This has something to with multi-cores CPU such as A53.
For SPL, we need to make sure the slave CPUs (CPU1/2/3) trapped in a 'place'
Where they could be 'activated' by kernel for multi-processor environment.
It means the kernel get to 'activate' the slave CPUs from master CPU (CPU0)
 U-Boot proper only run on master CPU (CPU0). The rest of slave CPUs
are trapped in the beginning of SPL waiting to be 'activated'
by kernel.

In U-Boot proper, only master CPU gets to run this code and it will just
do the basic GIC setup and skip the 'trap'. The 'trap' is to prevent the slave
CPUs from running the same SPL, ATF and U-Boot code as the master CPU in
parallel. Only single core (maser CPU) is needed for bootloaders and firmware.
> 
> > I don't think this can be done in C code but let me see what I can do
> > to further optimize the flow to reduce the ifdeffery.
> 
> That would be nice, thanks.
Marek Vasut Feb. 21, 2020, 4:26 p.m. UTC | #5
On 2/20/20 8:05 PM, Ang, Chee Hong wrote:
>> On 2/20/20 3:27 AM, Ang, Chee Hong wrote:
>>>> On 2/19/20 1:25 PM, chee.hong.ang at intel.com wrote:
>>>> [...]
>>>>> diff --git a/arch/arm/mach-socfpga/lowlevel_init.S
>>>>> b/arch/arm/mach-socfpga/lowlevel_init.S
>>>>> new file mode 100644
>>>>> index 0000000..68053a0
>>>>> --- /dev/null
>>>>> +++ b/arch/arm/mach-socfpga/lowlevel_init.S
>>>>
>>>> This should be some lowlevel_init_64.S to make it clear it's only for
>>>> arm64 platforms.
>>> OK. It makes sense. Thanks.
>>>>
>>>>> @@ -0,0 +1,85 @@
>>>>> +/* SPDX-License-Identifier: GPL-2.0 */
>>>>> +/*
>>>>> + * Copyright (C) 2019, Intel Corporation  */
>>>>> +
>>>>> +#include <asm-offsets.h>
>>>>> +#include <config.h>
>>>>> +#include <linux/linkage.h>
>>>>> +#include <asm/macro.h>
>>>>> +
>>>>> +ENTRY(lowlevel_init)
>>>>> +	mov	x29, lr			/* Save LR */
>>>>> +
>>>>> +#if defined(CONFIG_GICV2) || defined(CONFIG_GICV3) #ifdef
>>>>> +CONFIG_SPL_ATF
>>>>> +	branch_if_slave x0, 2f
>>>>> +#else
>>>>> +	branch_if_slave x0, 1f
>>>>> +#endif
>>>>> +
>>>>> +	ldr	x0, =GICD_BASE
>>>>> +	bl	gic_init_secure
>>>>> +#ifdef CONFIG_SPL_BUILD
>>>>> +	b	2f
>>>>> +#else
>>>>> +	b	3f
>>>>> +#endif
>>>>
>>>> Can't this be done in C code ? Can we reduce the ifdeffery ?
>>> This lowlevel_init function is shared by SPL and U-Boot and they run
>>> in slightly different flow.
>>
>> What does this 'different flow' mean ?
> This has something to with multi-cores CPU such as A53.
> For SPL, we need to make sure the slave CPUs (CPU1/2/3) trapped in a 'place'
> Where they could be 'activated' by kernel for multi-processor environment.
> It means the kernel get to 'activate' the slave CPUs from master CPU (CPU0)
>  U-Boot proper only run on master CPU (CPU0). The rest of slave CPUs
> are trapped in the beginning of SPL waiting to be 'activated'
> by kernel.

OK, so the secondary CPUs are spinning until the kernel releases them.

> In U-Boot proper, only master CPU gets to run this code and it will just
> do the basic GIC setup and skip the 'trap'. The 'trap' is to prevent the slave
> CPUs from running the same SPL, ATF and U-Boot code as the master CPU in
> parallel. Only single core (maser CPU) is needed for bootloaders and firmware.

I would expect all the other SMP platforms solved this issue with
secondary CPUs already, so why is agilex special ?
Ang, Chee Hong Feb. 21, 2020, 6:35 p.m. UTC | #6
> On 2/20/20 8:05 PM, Ang, Chee Hong wrote:
> >> On 2/20/20 3:27 AM, Ang, Chee Hong wrote:
> >>>> On 2/19/20 1:25 PM, chee.hong.ang at intel.com wrote:
> >>>> [...]
> >>>>> diff --git a/arch/arm/mach-socfpga/lowlevel_init.S
> >>>>> b/arch/arm/mach-socfpga/lowlevel_init.S
> >>>>> new file mode 100644
> >>>>> index 0000000..68053a0
> >>>>> --- /dev/null
> >>>>> +++ b/arch/arm/mach-socfpga/lowlevel_init.S
> >>>>
> >>>> This should be some lowlevel_init_64.S to make it clear it's only
> >>>> for
> >>>> arm64 platforms.
> >>> OK. It makes sense. Thanks.
> >>>>
> >>>>> @@ -0,0 +1,85 @@
> >>>>> +/* SPDX-License-Identifier: GPL-2.0 */
> >>>>> +/*
> >>>>> + * Copyright (C) 2019, Intel Corporation  */
> >>>>> +
> >>>>> +#include <asm-offsets.h>
> >>>>> +#include <config.h>
> >>>>> +#include <linux/linkage.h>
> >>>>> +#include <asm/macro.h>
> >>>>> +
> >>>>> +ENTRY(lowlevel_init)
> >>>>> +	mov	x29, lr			/* Save LR */
> >>>>> +
> >>>>> +#if defined(CONFIG_GICV2) || defined(CONFIG_GICV3) #ifdef
> >>>>> +CONFIG_SPL_ATF
> >>>>> +	branch_if_slave x0, 2f
> >>>>> +#else
> >>>>> +	branch_if_slave x0, 1f
> >>>>> +#endif
> >>>>> +
> >>>>> +	ldr	x0, =GICD_BASE
> >>>>> +	bl	gic_init_secure
> >>>>> +#ifdef CONFIG_SPL_BUILD
> >>>>> +	b	2f
> >>>>> +#else
> >>>>> +	b	3f
> >>>>> +#endif
> >>>>
> >>>> Can't this be done in C code ? Can we reduce the ifdeffery ?
> >>> This lowlevel_init function is shared by SPL and U-Boot and they run
> >>> in slightly different flow.
> >>
> >> What does this 'different flow' mean ?
> > This has something to with multi-cores CPU such as A53.
> > For SPL, we need to make sure the slave CPUs (CPU1/2/3) trapped in a 'place'
> > Where they could be 'activated' by kernel for multi-processor environment.
> > It means the kernel get to 'activate' the slave CPUs from master CPU
> > (CPU0)  U-Boot proper only run on master CPU (CPU0). The rest of slave
> > CPUs are trapped in the beginning of SPL waiting to be 'activated'
> > by kernel.
> 
> OK, so the secondary CPUs are spinning until the kernel releases them.
> 
> > In U-Boot proper, only master CPU gets to run this code and it will
> > just do the basic GIC setup and skip the 'trap'. The 'trap' is to
> > prevent the slave CPUs from running the same SPL, ATF and U-Boot code
> > as the master CPU in parallel. Only single core (maser CPU) is needed for
> bootloaders and firmware.
> 
> I would expect all the other SMP platforms solved this issue with secondary
> CPUs already, so why is agilex special ?
Nothing special. Just like other SMP platforms, I am just telling you the master CPU
always skips the 'spinning trap' in SPL and U-Boot proper.
Ang, Chee Hong Feb. 21, 2020, 6:46 p.m. UTC | #7
> > On 2/20/20 8:05 PM, Ang, Chee Hong wrote:
> > >> On 2/20/20 3:27 AM, Ang, Chee Hong wrote:
> > >>>> On 2/19/20 1:25 PM, chee.hong.ang at intel.com wrote:
> > >>>> [...]
> > >>>>> diff --git a/arch/arm/mach-socfpga/lowlevel_init.S
> > >>>>> b/arch/arm/mach-socfpga/lowlevel_init.S
> > >>>>> new file mode 100644
> > >>>>> index 0000000..68053a0
> > >>>>> --- /dev/null
> > >>>>> +++ b/arch/arm/mach-socfpga/lowlevel_init.S
> > >>>>
> > >>>> This should be some lowlevel_init_64.S to make it clear it's only
> > >>>> for
> > >>>> arm64 platforms.
> > >>> OK. It makes sense. Thanks.
> > >>>>
> > >>>>> @@ -0,0 +1,85 @@
> > >>>>> +/* SPDX-License-Identifier: GPL-2.0 */
> > >>>>> +/*
> > >>>>> + * Copyright (C) 2019, Intel Corporation  */
> > >>>>> +
> > >>>>> +#include <asm-offsets.h>
> > >>>>> +#include <config.h>
> > >>>>> +#include <linux/linkage.h>
> > >>>>> +#include <asm/macro.h>
> > >>>>> +
> > >>>>> +ENTRY(lowlevel_init)
> > >>>>> +	mov	x29, lr			/* Save LR */
> > >>>>> +
> > >>>>> +#if defined(CONFIG_GICV2) || defined(CONFIG_GICV3) #ifdef
> > >>>>> +CONFIG_SPL_ATF
> > >>>>> +	branch_if_slave x0, 2f
> > >>>>> +#else
> > >>>>> +	branch_if_slave x0, 1f
> > >>>>> +#endif
> > >>>>> +
> > >>>>> +	ldr	x0, =GICD_BASE
> > >>>>> +	bl	gic_init_secure
> > >>>>> +#ifdef CONFIG_SPL_BUILD
> > >>>>> +	b	2f
> > >>>>> +#else
> > >>>>> +	b	3f
> > >>>>> +#endif
> > >>>>
> > >>>> Can't this be done in C code ? Can we reduce the ifdeffery ?
> > >>> This lowlevel_init function is shared by SPL and U-Boot and they
> > >>> run in slightly different flow.
> > >>
> > >> What does this 'different flow' mean ?
> > > This has something to with multi-cores CPU such as A53.
> > > For SPL, we need to make sure the slave CPUs (CPU1/2/3) trapped in a 'place'
> > > Where they could be 'activated' by kernel for multi-processor environment.
> > > It means the kernel get to 'activate' the slave CPUs from master CPU
> > > (CPU0)  U-Boot proper only run on master CPU (CPU0). The rest of
> > > slave CPUs are trapped in the beginning of SPL waiting to be 'activated'
> > > by kernel.
> >
> > OK, so the secondary CPUs are spinning until the kernel releases them.
> >
> > > In U-Boot proper, only master CPU gets to run this code and it will
> > > just do the basic GIC setup and skip the 'trap'. The 'trap' is to
> > > prevent the slave CPUs from running the same SPL, ATF and U-Boot
> > > code as the master CPU in parallel. Only single core (maser CPU) is
> > > needed for
> > bootloaders and firmware.
> >
> > I would expect all the other SMP platforms solved this issue with
> > secondary CPUs already, so why is agilex special ?
> Nothing special. Just like other SMP platforms, I am just telling you the master
> CPU always skips the 'spinning trap' in SPL and U-Boot proper.
ATF is now used to activate the secondary CPUs by Linux.
This lowlevel_init is to make sure secondary CPUs trap in ATF
instead of SPL after ATF is initialized.
Marek Vasut Feb. 23, 2020, 1:59 p.m. UTC | #8
On 2/21/20 7:46 PM, Ang, Chee Hong wrote:
>>> On 2/20/20 8:05 PM, Ang, Chee Hong wrote:
>>>>> On 2/20/20 3:27 AM, Ang, Chee Hong wrote:
>>>>>>> On 2/19/20 1:25 PM, chee.hong.ang at intel.com wrote:
>>>>>>> [...]
>>>>>>>> diff --git a/arch/arm/mach-socfpga/lowlevel_init.S
>>>>>>>> b/arch/arm/mach-socfpga/lowlevel_init.S
>>>>>>>> new file mode 100644
>>>>>>>> index 0000000..68053a0
>>>>>>>> --- /dev/null
>>>>>>>> +++ b/arch/arm/mach-socfpga/lowlevel_init.S
>>>>>>>
>>>>>>> This should be some lowlevel_init_64.S to make it clear it's only
>>>>>>> for
>>>>>>> arm64 platforms.
>>>>>> OK. It makes sense. Thanks.
>>>>>>>
>>>>>>>> @@ -0,0 +1,85 @@
>>>>>>>> +/* SPDX-License-Identifier: GPL-2.0 */
>>>>>>>> +/*
>>>>>>>> + * Copyright (C) 2019, Intel Corporation  */
>>>>>>>> +
>>>>>>>> +#include <asm-offsets.h>
>>>>>>>> +#include <config.h>
>>>>>>>> +#include <linux/linkage.h>
>>>>>>>> +#include <asm/macro.h>
>>>>>>>> +
>>>>>>>> +ENTRY(lowlevel_init)
>>>>>>>> +	mov	x29, lr			/* Save LR */
>>>>>>>> +
>>>>>>>> +#if defined(CONFIG_GICV2) || defined(CONFIG_GICV3) #ifdef
>>>>>>>> +CONFIG_SPL_ATF
>>>>>>>> +	branch_if_slave x0, 2f
>>>>>>>> +#else
>>>>>>>> +	branch_if_slave x0, 1f
>>>>>>>> +#endif
>>>>>>>> +
>>>>>>>> +	ldr	x0, =GICD_BASE
>>>>>>>> +	bl	gic_init_secure
>>>>>>>> +#ifdef CONFIG_SPL_BUILD
>>>>>>>> +	b	2f
>>>>>>>> +#else
>>>>>>>> +	b	3f
>>>>>>>> +#endif
>>>>>>>
>>>>>>> Can't this be done in C code ? Can we reduce the ifdeffery ?
>>>>>> This lowlevel_init function is shared by SPL and U-Boot and they
>>>>>> run in slightly different flow.
>>>>>
>>>>> What does this 'different flow' mean ?
>>>> This has something to with multi-cores CPU such as A53.
>>>> For SPL, we need to make sure the slave CPUs (CPU1/2/3) trapped in a 'place'
>>>> Where they could be 'activated' by kernel for multi-processor environment.
>>>> It means the kernel get to 'activate' the slave CPUs from master CPU
>>>> (CPU0)  U-Boot proper only run on master CPU (CPU0). The rest of
>>>> slave CPUs are trapped in the beginning of SPL waiting to be 'activated'
>>>> by kernel.
>>>
>>> OK, so the secondary CPUs are spinning until the kernel releases them.
>>>
>>>> In U-Boot proper, only master CPU gets to run this code and it will
>>>> just do the basic GIC setup and skip the 'trap'. The 'trap' is to
>>>> prevent the slave CPUs from running the same SPL, ATF and U-Boot
>>>> code as the master CPU in parallel. Only single core (maser CPU) is
>>>> needed for
>>> bootloaders and firmware.
>>>
>>> I would expect all the other SMP platforms solved this issue with
>>> secondary CPUs already, so why is agilex special ?
>> Nothing special. Just like other SMP platforms, I am just telling you the master
>> CPU always skips the 'spinning trap' in SPL and U-Boot proper.
> ATF is now used to activate the secondary CPUs by Linux.
> This lowlevel_init is to make sure secondary CPUs trap in ATF
> instead of SPL after ATF is initialized. 

So that's what it is. OK, please document it in the commit message, thanks.
diff mbox series

Patch

diff --git a/arch/arm/mach-socfpga/Makefile b/arch/arm/mach-socfpga/Makefile
index 418f543..3310e92 100644
--- a/arch/arm/mach-socfpga/Makefile
+++ b/arch/arm/mach-socfpga/Makefile
@@ -29,6 +29,7 @@  endif
 
 ifdef CONFIG_TARGET_SOCFPGA_STRATIX10
 obj-y	+= clock_manager_s10.o
+obj-y	+= lowlevel_init.o
 obj-y	+= mailbox_s10.o
 obj-y	+= misc_s10.o
 obj-y	+= mmu-arm64_s10.o
@@ -41,6 +42,7 @@  endif
 
 ifdef CONFIG_TARGET_SOCFPGA_AGILEX
 obj-y	+= clock_manager_agilex.o
+obj-y	+= lowlevel_init.o
 obj-y	+= mailbox_s10.o
 obj-y	+= misc_s10.o
 obj-y	+= mmu-arm64_s10.o
diff --git a/arch/arm/mach-socfpga/lowlevel_init.S b/arch/arm/mach-socfpga/lowlevel_init.S
new file mode 100644
index 0000000..68053a0
--- /dev/null
+++ b/arch/arm/mach-socfpga/lowlevel_init.S
@@ -0,0 +1,85 @@ 
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2019, Intel Corporation
+ */
+
+#include <asm-offsets.h>
+#include <config.h>
+#include <linux/linkage.h>
+#include <asm/macro.h>
+
+ENTRY(lowlevel_init)
+	mov	x29, lr			/* Save LR */
+
+#if defined(CONFIG_GICV2) || defined(CONFIG_GICV3)
+#ifdef CONFIG_SPL_ATF
+	branch_if_slave x0, 2f
+#else
+	branch_if_slave x0, 1f
+#endif
+
+	ldr	x0, =GICD_BASE
+	bl	gic_init_secure
+#ifdef CONFIG_SPL_BUILD
+	b	2f
+#else
+	b	3f
+#endif
+
+1:
+#if defined(CONFIG_GICV3)
+	ldr	x0, =GICR_BASE
+	bl	gic_init_secure_percpu
+#elif defined(CONFIG_GICV2)
+	ldr	x0, =GICD_BASE
+	ldr	x1, =GICC_BASE
+	bl	gic_init_secure_percpu
+#endif
+#endif
+
+#ifdef CONFIG_ARMV8_MULTIENTRY
+	branch_if_master x0, x1, 3f
+
+	/*
+	 * Slave should wait for master clearing spin table.
+	 * This sync prevent slaves observing incorrect
+	 * value of spin table and jumping to wrong place.
+	 */
+#if defined(CONFIG_GICV2) || defined(CONFIG_GICV3)
+#ifdef CONFIG_GICV2
+	ldr	x0, =GICC_BASE
+#endif
+	bl	gic_wait_for_interrupt
+#endif
+
+	/*
+	 * All slaves will enter EL2 and optionally EL1.
+	 */
+	adr	x4, lowlevel_in_el2
+	ldr	x5, =ES_TO_AARCH64
+	bl	armv8_switch_to_el2
+
+lowlevel_in_el2:
+#ifdef CONFIG_ARMV8_SWITCH_TO_EL1
+	adr	x4, lowlevel_in_el1
+	ldr	x5, =ES_TO_AARCH64
+	bl	armv8_switch_to_el1
+
+lowlevel_in_el1:
+#endif
+#endif /* CONFIG_ARMV8_MULTIENTRY */
+
+2:
+#ifdef CONFIG_SPL_BUILD
+	ldr	x4, =CPU_RELEASE_ADDR
+	ldr	x5, [x4]
+	cbz	x5, checkslavecpu
+	br	x5
+checkslavecpu:
+	branch_if_slave x0, 2b
+#endif
+
+3:
+	mov	lr, x29			/* Restore LR */
+	ret
+ENDPROC(lowlevel_init)