1/*
2 * Copyright 2020, Data61, CSIRO (ABN 41 687 119 230)
3 *
4 * SPDX-License-Identifier: GPL-2.0-only
5 */
6
7#include <autoconf.h>
8#include <elfloader/gen_config.h>
9
10#include <assembler.h>
11#include <armv/assembler.h>
12
13.extern non_boot_core
14
15#if CONFIG_MAX_NUM_NODES > 1
16BEGIN_FUNC(imx_non_boot)
17    /* Invalidate caches before proceeding... */
18    mov     r0, #0
19    mcr     IIALL(r0)
20    dcache  isw
21
22    b       non_boot_core
23END_FUNC(imx_non_boot)
24
25BEGIN_FUNC(non_boot_core)
26    /* Disable Async aborts that might be pending from bootloader */
27    cpsid   ifa
28
29    /* Enable SMP */
30    mrc     ACTLR(r0)
31    orr     r0, r0, #(1 << 6)       /* enable SMP bit   */
32#ifdef CONFIG_ARM_CORTEX_A9
33    orr     r0, r0, #1              /* enable FW bit    */
34#endif
35    mcr     ACTLR(r0)
36
37    ldr     r2, =smp_aps_index
38    ldr     r3, =core_stack_alloc - 0x10
39
40    /* Get stack index */
41    mov     r0, r2
421:  ldrex   r1, [r0]
43    add     r1, r1, #1
44    strex   r2, r1, [r0]
45    teq     r2, #0
46    bne     1b
47
48    /* Set up stack */
49    mov     r0, #0x1000
50    mul     r1, r0
51    add     r3, r1
52    mov     sp, r3
53    b       non_boot_main
54END_FUNC(non_boot_core)
55#endif /* CONFIG_MAX_NUM_NODES */
56
57