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#include <elfloader.h>
10#include <printf.h>
11
12#define IMX6_SCU_PADDR          0x00a00000
13#define IMX6_SCU_SACR_PADDR     (IMX6_SCU_PADDR + 0x50)
14#define IMX6_SCU_NSACR_PADDR    (IMX6_SCU_PADDR + 0x54)
15#define IMX6_CSU_PADDR          0x021c0000
16#define IMX6_CSU_SIZE           160
17#define IMX6_GICD_PADDR         0x00a01000
18#define IMX6_GICC_PADDR         0x00a00100
19
20/* non-secure bit: 0 secure; 1 nonsecure */
21#define SCR_NS      (0)
22
23/* controls which mode takes IRQ exceptions: 0 IRQ mode; 1 monitor mode */
24#define SCR_IRQ     (1)
25
26/* FIQ mode control */
27#define SCR_FIQ     (2)
28
29/* external abort handler. 0 abort mode; 1 monitor mode */
30#define SCR_EA      (3)
31
32/* CPSR.F can be modified in nonsecure mode */
33#define SCR_FW      (4)
34
35/* CPSR.A can be modified in nonsecure mode */
36#define SCR_AW      (5)
37
38/* not early terminination. not implmented */
39#define SCR_NET     (6)
40
41/* secure monitor call disabled: 0 smc executes in nonsecure state;
42 * 1 undefined instruction in nonsecure state
43 */
44#define SCR_SCD     (7)
45
46/* secure instruction fetch. when in secure state, the bit disables
47 * instruction fetches from non-secure memory */
48#define SCR_SIF     (9)
49
50#define MONITOR_MODE        (0x16)
51#define SUPERVISOR_MODE     (0x13)
52
53static int mon_init_done = 0;
54
55void arm_halt(void)
56{
57    while (1) {
58        asm volatile("wfe");
59    }
60}
61
62void check_mode(void)
63{
64    uint32_t cpsr = 0;
65    asm volatile("mrs %0, cpsr":"=r"(cpsr));
66    printf("CPSR is %x\n", cpsr);
67}
68
69asm(".arch_extension sec\n");
70
71#ifndef CONFIG_ARM_S_SUPERVISOR_MODE
72UNUSED static void switch_to_mon_mode(void)
73{
74    if (mon_init_done == 0) {
75        /* first need to make sure that we are in secure world */
76        uint32_t scr = 0;
77        /* read the secure configuration register, note if we are
78         * in nonsecure world, the instruction fails.
79         */
80
81        asm volatile("mrc p15, 0, %0, c1, c1, 0":"=r"(scr));
82
83        if (scr & BIT(SCR_NS)) {
84            printf("In nonsecure world, you should never see this!\n");
85            arm_halt();
86        }
87
88        check_mode();
89
90        /* now switch to secure monitor mode */
91        asm volatile("mov r8, sp\n\t"
92                     "cps %0\n\t"
93                     "isb\n"
94                     "mov sp, r8\n\t"
95                     ::"I"(MONITOR_MODE));
96        mon_init_done = 1;
97        check_mode();
98        printf("ELF loader: monitor mode init done\n");
99    }
100}
101#endif
102
103#ifdef CONFIG_ARM_MONITOR_HOOK
104
105#error please ensure the MON_VECTOR_START is not used by the kernel.
106
107/* The physical address region [MON_VECTOR_START, MON_VECTOR_START + size)
108 * must not be used by the seL4 kernel. The VECTOR_BASE must be
109 * the same as MON_VECTOR_START */
110
111#define MON_VECTOR_START    (0x10000000)
112extern void arm_monitor_vector(void);
113extern void arm_monitor_vector_end(void);
114extern void *memcpy(void *dest, void *src, size_t n);
115
116static void install_monitor_hook(void)
117{
118    uint32_t size = arm_monitor_vector_end - arm_monitor_vector;
119    switch_to_mon_mode();
120    printf("Copy monitor mode vector from %x to %x size %x\n", (arm_monitor_vector), MON_VECTOR_START, size);
121    memcpy((void *)MON_VECTOR_START, (void *)(arm_monitor_vector), size);
122    asm volatile("dmb\n isb\n");
123    asm volatile("mcr p15, 0, %0, c12, c0, 1"::"r"(MON_VECTOR_START));
124}
125#endif /* end of CONFIG_ARM_MONITOR_HOOK */
126
127#ifdef CONFIG_ARM_NS_SUPERVISOR_MODE
128static void enable_ns_access_cp(void)
129{
130    uint32_t nsacr = 0;
131    asm volatile("mrc p15, 0, %0, c1, c1, 2":"=r"(nsacr));
132
133    /* enable cp10, cp11, TL, and PLE access */
134    nsacr |= BIT(10) |  BIT(11) | BIT(17) | BIT(16);
135    asm volatile("mcr p15, 0, %0, c1, c1, 2"::"r"(nsacr));
136}
137
138struct gicd_map {
139    uint32_t enable;
140    uint32_t ic_type;
141    uint32_t dist_ident;
142    uint32_t res1[29];
143    uint32_t security[32];
144    uint32_t enable_set[32];
145    uint32_t enable_clr[32];
146    uint32_t pending_set[32];
147    uint32_t pending_clr[32];
148    uint32_t active[32];
149    uint32_t res2[32];
150    uint32_t priority[255];
151};
152
153struct gicc_map {
154    uint32_t ctrl;
155    uint32_t pri_mask;
156    uint32_t pb_c;
157    uint32_t int_ack;
158    uint32_t eoi;
159};
160
161volatile struct gicd_map *gicd = (volatile struct gicd_map *)(IMX6_GICD_PADDR);
162volatile struct gicc_map *gicc = (volatile struct gicc_map *)(IMX6_GICC_PADDR);
163
164static void route_irqs_to_nonsecure(void)
165{
166    int i = 0;
167    int nirqs = 32 * ((gicd->ic_type & 0x1f) + 1);
168    printf("Number of IRQs: %d\n", nirqs);
169    gicd->enable = 0;
170
171    /* note: the security and priority initialisations in
172     * non-secure mode will not work, but use the values
173     * set by secure mode.
174     */
175
176    /* set all irqs to group 1 - nonsecure */
177    for (i = 0; i < nirqs; i += 32) {
178        gicd->security[i >> 5] = 0xffffffff;
179    }
180
181    /* assign the irqs in a single priority group: no preemptions */
182    for (i = 0; i < nirqs; i += 4) {
183        gicd->priority[i >> 2] = 0x80808080;
184    }
185
186    gicc->ctrl = 0;
187
188    /* writing 255 always set the largest (lowest) priority value.
189     * missing this hurts health */
190    gicc->pri_mask = 0xff;
191}
192
193/* enable nonsecure access of the I/O devices */
194static void set_csu(void)
195{
196    volatile uint32_t *addr = (volatile uint32_t *)IMX6_CSU_PADDR;
197    uint32_t size = 0;
198
199    while (size < IMX6_CSU_SIZE / sizeof(uint32_t)) {
200        *addr = 0x00ff00ff;
201        asm volatile("dsb");
202        addr++;
203        size++;
204    }
205
206    /* please check the rest of CSU registers if some
207     * devices do not work. See the Security Reference
208     * Manual for i.MX6. */
209}
210
211static void set_smp_bit(void)
212{
213    uint32_t acr = 0;
214    uint32_t nsacr = 0;
215    asm volatile("mrc p15, 0, %0, c1, c0, 1":"=r"(acr));
216    acr |= BIT(6);
217    asm volatile("mcr p15, 0, %0, c1, c0, 1"::"r"(acr));
218
219    /* allow nonsecure to change smp bit */
220    asm volatile("mrc p15, 0, %0, c1, c1, 2":"=r"(nsacr));
221    nsacr |= BIT(18);
222    asm volatile("mcr p15, 0, %0, c1, c1, 2"::"r"(nsacr));
223
224}
225
226/* give access to the SCU registers for all cores in nonsecure world */
227static void enable_scu_ns_access(void)
228{
229    *((volatile uint32_t *)(IMX6_SCU_SACR_PADDR)) = 0xf;
230    *((volatile uint32_t *)(IMX6_SCU_NSACR_PADDR)) = 0x1fff;
231
232}
233#endif /* end of CONFIG_ARM_NS_SUPERVISOR_MODE */
234
235/* the elfloader put us in secure svc mode */
236void platform_init(void)
237{
238    mon_init_done = 0;
239
240#ifdef CONFIG_ARM_MONITOR_HOOK
241    install_monitor_hook();
242#endif
243
244#ifdef CONFIG_ARM_NS_SUPERVISOR_MODE
245    /* if the image is binary, the mon_init_done is not properly initialised */
246    switch_to_mon_mode();
247    enable_scu_ns_access();
248    enable_ns_access_cp();
249    set_smp_bit();
250    set_csu();
251    route_irqs_to_nonsecure();
252    /* ignore the name, we switch to nonsecure supervisor mode */
253    asm volatile("push {r0, r1}              \n\t"
254                 "mov  r0, sp                \n\t"
255                 "mov  r1, #1                \n\t"
256                 "mcr  p15, 0, r1, c1, c1, 0 \n\t"
257                 "isb                        \n\t"
258                 "ldr  r1, =0x1d3            \n\t"
259                 "msr  spsr_cxfs, r1         \n\t"
260                 "ldr  lr, =mode_switch      \n\t"
261                 "movs pc, lr                \n\t"
262                 "mode_switch:               \n\t"
263                 "isb                        \n\t"
264                 "mov  sp, r0                \n\t"
265                 "pop  {r0, r1}              \n\t"
266                );
267
268    return;
269#endif
270#ifdef CONFIG_ARM_MONITOR_MODE
271    switch_to_mon_mode();
272#endif
273}
274