optee.c 4.9 KB

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