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