lib/optee/optee.c

   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
  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
  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)
  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
 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