1 /*
2 * Copyright (c) 2022 Google Inc. All rights reserved
3 *
4 * Permission is hereby granted, free of charge, to any person obtaining
5 * a copy of this software and associated documentation files
6 * (the "Software"), to deal in the Software without restriction,
7 * including without limitation the rights to use, copy, modify, merge,
8 * publish, distribute, sublicense, and/or sell copies of the Software,
9 * and to permit persons to whom the Software is furnished to do so,
10 * subject to the following conditions:
11 *
12 * The above copyright notice and this permission notice shall be
13 * included in all copies or substantial portions of the Software.
14 *
15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
16 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
17 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
18 * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
19 * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
20 * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
21 * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
22 */
23
24 #include <lib/dtb_embedded/dtb_embedded.h>
25 #include <lib/dtb_service/dtb_service.h>
26 #include <lib/ktipc/ktipc.h>
27 #include <libfdt.h>
28 #include <lk/init.h>
29 #include <lk/trace.h>
30 #include <uapi/uapi/err.h>
31
32 #define DT_PORT "com.android.kernel.device_tree"
33 #define DTB_PORT "com.android.trusty.kernel.device_tree.blob"
34
35 static struct ktipc_server dtb_ktipc_server =
36 KTIPC_SERVER_INITIAL_VALUE(dtb_ktipc_server, "dtb_ktipc_server");
37
find_dtb_by_compatible(const char * compat,const void ** dtb,size_t * dtb_size)38 static int find_dtb_by_compatible(const char* compat,
39 const void** dtb,
40 size_t* dtb_size) {
41 assert(dtb);
42 assert(dtb_size);
43
44 int rc;
45 const void* current_dtb = NULL;
46 size_t current_dtb_size = 0;
47 struct dtb_embedded_iterator* iter;
48 rc = dtb_embedded_iterator_new(&iter);
49 if (rc != NO_ERROR) {
50 return rc;
51 }
52
53 for (;;) {
54 rc = dtb_embedded_iterator_next(iter, ¤t_dtb, ¤t_dtb_size);
55 if (rc != NO_ERROR) {
56 break;
57 }
58 int test_node_offset = fdt_node_offset_by_compatible(
59 current_dtb, -1 /* search from the start of the dtb */, compat);
60 if (test_node_offset >= 0) {
61 /* Found the test node */
62 *dtb = current_dtb;
63 *dtb_size = current_dtb_size;
64 break;
65 }
66 }
67
68 dtb_embedded_iterator_free(&iter);
69
70 return rc;
71 }
72
apply_test_overlay(const void * base_dtb,size_t base_dtb_size,const void * test_dtbo,size_t test_dtbo_size)73 static void* apply_test_overlay(const void* base_dtb,
74 size_t base_dtb_size,
75 const void* test_dtbo,
76 size_t test_dtbo_size) {
77 /* Overestimate the size needed for the full dtb for now */
78 size_t full_size = base_dtb_size + test_dtbo_size;
79 void* base = calloc(1, full_size);
80 if (!base) {
81 goto alloc_base;
82 }
83 /*
84 * libfdt overwrites part of the dtbo header if applying the overlay fails
85 * so we have to make a temporary copy since the original dtbo is in
86 * read-only memory.
87 */
88 void* overlay = calloc(1, test_dtbo_size);
89 if (!overlay) {
90 goto alloc_overlay;
91 }
92 /*
93 * Using fdt_open_into copies the dtb to the buffer allocated above and
94 * updates the fdt header with the new buffer size
95 */
96 int rc = fdt_open_into(base_dtb, base, full_size);
97 if (rc < 0) {
98 TRACEF("fdt_open_into failed for base dtb (%d)\n", rc);
99 goto copy_base;
100 }
101 rc = fdt_open_into(test_dtbo, overlay, test_dtbo_size);
102 if (rc < 0) {
103 TRACEF("fdt_open_into failed for test dtb overlay (%d)\n", rc);
104 goto copy_overlay;
105 }
106 rc = fdt_overlay_apply(base, overlay);
107 if (rc < 0) {
108 TRACEF("Failed to apply test overlay to base dtb (%d)\n", rc);
109 goto apply_overlay;
110 }
111 free(overlay);
112 return base;
113
114 apply_overlay:
115 copy_overlay:
116 copy_base:
117 free(overlay);
118 alloc_overlay:
119 free(base);
120 alloc_base:
121 return NULL;
122 }
123
platform_dtb_init(uint level)124 static void platform_dtb_init(uint level) {
125 int rc;
126 rc = ktipc_server_start(&dtb_ktipc_server);
127 if (rc < 0) {
128 panic("Failed (%d) to start dtb server\n", rc);
129 }
130
131 const void* base_dtb = NULL;
132 const void* test_dtbo = NULL;
133 size_t base_dtb_size, test_dtbo_size = 0;
134 rc = find_dtb_by_compatible("google,test_base", &base_dtb, &base_dtb_size);
135 if (rc != NO_ERROR) {
136 panic("No embedded base dtb found (%d)\n", rc);
137 }
138
139 rc = find_dtb_by_compatible("google,test_overlay", &test_dtbo,
140 &test_dtbo_size);
141 if (rc != NO_ERROR) {
142 panic("No embedded test dtbo found (%d)\n", rc);
143 }
144
145 const void* dtb = apply_test_overlay(base_dtb, base_dtb_size, test_dtbo,
146 test_dtbo_size);
147 if (!dtb) {
148 panic("Failed to apply test overlay to base dtb\n");
149 }
150 size_t dtb_size = fdt_totalsize(dtb);
151
152 rc = dtb_service_add(dtb, dtb_size, DT_PORT, DTB_PORT, &dtb_ktipc_server);
153 if (rc < 0) {
154 panic("Failed (%d) to add dtb service to server\n", rc);
155 }
156 }
157
158 LK_INIT_HOOK(platform_dtb, platform_dtb_init, LK_INIT_LEVEL_THREADING + 1);
159