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