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