1 // SPDX-License-Identifier: GPL-2.0+
2 /*
3 * Copyright (C) 2016 Freescale Semiconductor, Inc.
4 */
5
6 #include <common.h>
7 #include <log.h>
8 #include <asm/io.h>
9 #include <asm/mach-imx/sys_proto.h>
10 #include <command.h>
11 #include <elf.h>
12 #include <imx_sip.h>
13 #include <linux/arm-smccc.h>
14 #include <linux/compiler.h>
15 #include <cpu_func.h>
16
17 /* Just to avoid build error */
18 #if IS_ENABLED(CONFIG_IMX8M)
19 #define SRC_M4C_NON_SCLR_RST_MASK BIT(0)
20 #define SRC_M4_ENABLE_MASK BIT(0)
21 #define SRC_M4_REG_OFFSET 0
22 #endif
23
imx_bootaux_get_hostmap(void)24 __weak const struct rproc_att *imx_bootaux_get_hostmap(void)
25 {
26 return NULL;
27 }
28
get_host_mapping(unsigned long auxcore)29 static const struct rproc_att *get_host_mapping(unsigned long auxcore)
30 {
31 const struct rproc_att *mmap = imx_bootaux_get_hostmap();
32
33 while (mmap && mmap->size) {
34 if (mmap->da <= auxcore &&
35 mmap->da + mmap->size > auxcore)
36 return mmap;
37 mmap++;
38 }
39
40 return NULL;
41 }
42
43 /*
44 * A very simple elf loader for the auxilary core, assumes the image
45 * is valid, returns the entry point address.
46 * Translates load addresses in the elf file to the U-Boot address space.
47 */
load_elf_image_m_core_phdr(unsigned long addr,ulong * stack)48 static unsigned long load_elf_image_m_core_phdr(unsigned long addr, ulong *stack)
49 {
50 Elf32_Ehdr *ehdr; /* ELF header structure pointer */
51 Elf32_Phdr *phdr; /* Program header structure pointer */
52 int num = 0;
53 int i;
54
55 ehdr = (Elf32_Ehdr *)addr;
56 phdr = (Elf32_Phdr *)(addr + ehdr->e_phoff);
57
58 /* Load each program header */
59 for (i = 0; i < ehdr->e_phnum; ++i, ++phdr) {
60 const struct rproc_att *mmap = get_host_mapping(phdr->p_paddr);
61 void *dst, *src;
62
63 if (phdr->p_type != PT_LOAD)
64 continue;
65
66 if (!mmap) {
67 printf("Invalid aux core address: %08x\n",
68 phdr->p_paddr);
69 return 0;
70 }
71
72 dst = (void *)(ulong)(phdr->p_paddr - mmap->da) + mmap->sa;
73 src = (void *)addr + phdr->p_offset;
74
75 debug("Loading phdr %i to 0x%p (%i bytes)\n",
76 i, dst, phdr->p_filesz);
77
78 if (phdr->p_filesz) {
79 memcpy(dst, src, phdr->p_filesz);
80 /* Stack in __isr_vector is the first section/word */
81 if (!num)
82 *stack = *(uint32_t *)src;
83 num++;
84 }
85 if (phdr->p_filesz != phdr->p_memsz)
86 memset(dst + phdr->p_filesz, 0x00,
87 phdr->p_memsz - phdr->p_filesz);
88 flush_cache((unsigned long)dst &
89 ~(CONFIG_SYS_CACHELINE_SIZE - 1),
90 ALIGN(phdr->p_filesz, CONFIG_SYS_CACHELINE_SIZE));
91 }
92
93 return ehdr->e_entry;
94 }
95
arch_auxiliary_core_up(u32 core_id,ulong addr)96 int arch_auxiliary_core_up(u32 core_id, ulong addr)
97 {
98 ulong stack, pc;
99
100 if (!addr)
101 return -EINVAL;
102
103 /*
104 * handling ELF64 binaries
105 * isn't supported yet.
106 */
107 if (valid_elf_image(addr)) {
108 pc = load_elf_image_m_core_phdr(addr, &stack);
109 if (!pc)
110 return CMD_RET_FAILURE;
111
112 if (!IS_ENABLED(CONFIG_ARM64))
113 stack = 0x0;
114 } else {
115 /*
116 * Assume binary file with vector table at the beginning.
117 * Cortex-M4 vector tables start with the stack pointer (SP)
118 * and reset vector (initial PC).
119 */
120 stack = *(u32 *)addr;
121 pc = *(u32 *)(addr + 4);
122 }
123
124 printf("## Starting auxiliary core stack = 0x%08lX, pc = 0x%08lX...\n",
125 stack, pc);
126
127 /* Set the stack and pc to M4 bootROM */
128 writel(stack, M4_BOOTROM_BASE_ADDR);
129 writel(pc, M4_BOOTROM_BASE_ADDR + 4);
130
131 flush_dcache_all();
132
133 /* Enable M4 */
134 if (IS_ENABLED(CONFIG_IMX8M)) {
135 arm_smccc_smc(IMX_SIP_SRC, IMX_SIP_SRC_M4_START, 0, 0, 0, 0, 0, 0, NULL);
136 } else {
137 clrsetbits_le32(SRC_BASE_ADDR + SRC_M4_REG_OFFSET,
138 SRC_M4C_NON_SCLR_RST_MASK, SRC_M4_ENABLE_MASK);
139 }
140
141 return 0;
142 }
143
arch_auxiliary_core_check_up(u32 core_id)144 int arch_auxiliary_core_check_up(u32 core_id)
145 {
146 struct arm_smccc_res res;
147 unsigned int val;
148
149 if (IS_ENABLED(CONFIG_IMX8M)) {
150 arm_smccc_smc(IMX_SIP_SRC, IMX_SIP_SRC_M4_STARTED, 0, 0, 0, 0, 0, 0, &res);
151 return res.a0;
152 }
153
154 val = readl(SRC_BASE_ADDR + SRC_M4_REG_OFFSET);
155
156 if (val & SRC_M4C_NON_SCLR_RST_MASK)
157 return 0; /* assert in reset */
158
159 return 1;
160 }
161
162 /*
163 * To i.MX6SX and i.MX7D, the image supported by bootaux needs
164 * the reset vector at the head for the image, with SP and PC
165 * as the first two words.
166 *
167 * Per the cortex-M reference manual, the reset vector of M4 needs
168 * to exist at 0x0 (TCMUL). The PC and SP are the first two addresses
169 * of that vector. So to boot M4, the A core must build the M4's reset
170 * vector with getting the PC and SP from image and filling them to
171 * TCMUL. When M4 is kicked, it will load the PC and SP by itself.
172 * The TCMUL is mapped to (M4_BOOTROM_BASE_ADDR) at A core side for
173 * accessing the M4 TCMUL.
174 */
do_bootaux(struct cmd_tbl * cmdtp,int flag,int argc,char * const argv[])175 static int do_bootaux(struct cmd_tbl *cmdtp, int flag, int argc,
176 char *const argv[])
177 {
178 ulong addr;
179 int ret, up;
180
181 if (argc < 2)
182 return CMD_RET_USAGE;
183
184 up = arch_auxiliary_core_check_up(0);
185 if (up) {
186 printf("## Auxiliary core is already up\n");
187 return CMD_RET_SUCCESS;
188 }
189
190 addr = hextoul(argv[1], NULL);
191
192 if (!addr)
193 return CMD_RET_FAILURE;
194
195 ret = arch_auxiliary_core_up(0, addr);
196 if (ret)
197 return CMD_RET_FAILURE;
198
199 return CMD_RET_SUCCESS;
200 }
201
202 U_BOOT_CMD(
203 bootaux, CONFIG_SYS_MAXARGS, 1, do_bootaux,
204 "Start auxiliary core",
205 ""
206 );
207