1 // SPDX-License-Identifier: GPL-2.0+
2 /*
3 * Copyright (C) 2017 Linaro
4 * Bryan O'Donoghue <bryan.odonoghue@linaro.org>
5 */
6
7 #include <common.h>
8 #include <fdtdec.h>
9 #include <image.h>
10 #include <log.h>
11 #include <malloc.h>
12 #include <linux/libfdt.h>
13 #include <tee/optee.h>
14
15 #define optee_hdr_err_msg \
16 "OPTEE verification error:" \
17 "\n\thdr=%p image=0x%08lx magic=0x%08x tzdram 0x%08lx-0x%08lx " \
18 "\n\theader lo=0x%08x hi=0x%08x size=0x%08lx arch=0x%08x" \
19 "\n\tuimage params 0x%08lx-0x%08lx\n"
20
optee_verify_image(struct optee_header * hdr,unsigned long tzdram_start,unsigned long tzdram_len,unsigned long image_len)21 int optee_verify_image(struct optee_header *hdr, unsigned long tzdram_start,
22 unsigned long tzdram_len, unsigned long image_len)
23 {
24 unsigned long tzdram_end = tzdram_start + tzdram_len;
25 uint32_t tee_file_size;
26
27 tee_file_size = hdr->init_size + hdr->paged_size +
28 sizeof(struct optee_header);
29
30 if (hdr->magic != OPTEE_MAGIC ||
31 hdr->version != OPTEE_VERSION ||
32 hdr->init_load_addr_hi > tzdram_end ||
33 hdr->init_load_addr_lo < tzdram_start ||
34 tee_file_size > tzdram_len ||
35 tee_file_size != image_len ||
36 (hdr->init_load_addr_lo + tee_file_size) > tzdram_end) {
37 return -EINVAL;
38 }
39
40 return 0;
41 }
42
optee_verify_bootm_image(unsigned long image_addr,unsigned long image_load_addr,unsigned long image_len)43 int optee_verify_bootm_image(unsigned long image_addr,
44 unsigned long image_load_addr,
45 unsigned long image_len)
46 {
47 struct optee_header *hdr = (struct optee_header *)image_addr;
48 unsigned long tzdram_start = CONFIG_OPTEE_TZDRAM_BASE;
49 unsigned long tzdram_len = CONFIG_OPTEE_TZDRAM_SIZE;
50
51 int ret;
52
53 ret = optee_verify_image(hdr, tzdram_start, tzdram_len, image_len);
54 if (ret)
55 goto error;
56
57 if (image_load_addr + sizeof(*hdr) != hdr->init_load_addr_lo) {
58 ret = -EINVAL;
59 goto error;
60 }
61
62 return ret;
63 error:
64 printf(optee_hdr_err_msg, hdr, image_addr, hdr->magic, tzdram_start,
65 tzdram_start + tzdram_len, hdr->init_load_addr_lo,
66 hdr->init_load_addr_hi, image_len, hdr->arch, image_load_addr,
67 image_load_addr + image_len);
68
69 return ret;
70 }
71
72 #if defined(CONFIG_OF_LIBFDT)
optee_copy_firmware_node(const void * old_blob,void * fdt_blob)73 static int optee_copy_firmware_node(const void *old_blob, void *fdt_blob)
74 {
75 int old_offs, offs, ret, len;
76 const void *prop;
77
78 old_offs = fdt_path_offset(old_blob, "/firmware/optee");
79 if (old_offs < 0) {
80 debug("Original OP-TEE Device Tree node not found");
81 return old_offs;
82 }
83
84 offs = fdt_path_offset(fdt_blob, "/firmware");
85 if (offs < 0) {
86 offs = fdt_path_offset(fdt_blob, "/");
87 if (offs < 0)
88 return offs;
89
90 offs = fdt_add_subnode(fdt_blob, offs, "firmware");
91 if (offs < 0)
92 return offs;
93 }
94
95 offs = fdt_add_subnode(fdt_blob, offs, "optee");
96 if (offs < 0)
97 return offs;
98
99 /* copy the compatible property */
100 prop = fdt_getprop(old_blob, old_offs, "compatible", &len);
101 if (!prop) {
102 debug("missing OP-TEE compatible property");
103 return -EINVAL;
104 }
105
106 ret = fdt_setprop(fdt_blob, offs, "compatible", prop, len);
107 if (ret < 0)
108 return ret;
109
110 /* copy the method property */
111 prop = fdt_getprop(old_blob, old_offs, "method", &len);
112 if (!prop) {
113 debug("missing OP-TEE method property");
114 return -EINVAL;
115 }
116
117 ret = fdt_setprop(fdt_blob, offs, "method", prop, len);
118 if (ret < 0)
119 return ret;
120
121 return 0;
122 }
123
optee_copy_fdt_nodes(const void * old_blob,void * new_blob)124 int optee_copy_fdt_nodes(const void *old_blob, void *new_blob)
125 {
126 int nodeoffset, subnode, ret;
127 struct fdt_resource res;
128
129 if (fdt_check_header(old_blob))
130 return -EINVAL;
131
132 if (fdt_check_header(new_blob))
133 return -EINVAL;
134
135 /* only proceed if there is an /firmware/optee node */
136 if (fdt_path_offset(old_blob, "/firmware/optee") < 0) {
137 debug("No OP-TEE firmware node in old fdt, nothing to do");
138 return 0;
139 }
140
141 /*
142 * Do not proceed if the target dt already has an OP-TEE node.
143 * In this case assume that the system knows better somehow,
144 * so do not interfere.
145 */
146 if (fdt_path_offset(new_blob, "/firmware/optee") >= 0) {
147 debug("OP-TEE Device Tree node already exists in target");
148 return 0;
149 }
150
151 ret = optee_copy_firmware_node(old_blob, new_blob);
152 if (ret < 0) {
153 printf("Failed to add OP-TEE firmware node\n");
154 return ret;
155 }
156
157 /* optee inserts its memory regions as reserved-memory nodes */
158 nodeoffset = fdt_subnode_offset(old_blob, 0, "reserved-memory");
159 if (nodeoffset >= 0) {
160 for (subnode = fdt_first_subnode(old_blob, nodeoffset);
161 subnode >= 0;
162 subnode = fdt_next_subnode(old_blob, subnode)) {
163 const char *name = fdt_get_name(old_blob,
164 subnode, NULL);
165 if (!name)
166 return -EINVAL;
167
168 /* only handle optee reservations */
169 if (strncmp(name, "optee", 5))
170 continue;
171
172 /* check if this subnode has a reg property */
173 ret = fdt_get_resource(old_blob, subnode, "reg", 0,
174 &res);
175 if (!ret) {
176 struct fdt_memory carveout = {
177 .start = res.start,
178 .end = res.end,
179 };
180 char *oldname, *nodename, *tmp;
181
182 oldname = strdup(name);
183 if (!oldname)
184 return -ENOMEM;
185
186 tmp = oldname;
187 nodename = strsep(&tmp, "@");
188 if (!nodename) {
189 free(oldname);
190 return -EINVAL;
191 }
192
193 ret = fdtdec_add_reserved_memory(new_blob,
194 nodename,
195 &carveout,
196 NULL, true);
197 free(oldname);
198
199 if (ret < 0)
200 return ret;
201 }
202 }
203 }
204
205 return 0;
206 }
207 #endif
208