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