1
2
3
4
5
6#include <libfdt_env.h>
7#include <fdt.h>
8#include <libfdt.h>
9
10#include "libfdt_internal.h"
11
12static int _fdt_sw_check_header(void *fdt)
13{
14 if (fdt_magic(fdt) != FDT_SW_MAGIC)
15 return -FDT_ERR_BADMAGIC;
16
17 return 0;
18}
19
20#define FDT_SW_CHECK_HEADER(fdt) \
21 { \
22 int err; \
23 if ((err = _fdt_sw_check_header(fdt)) != 0) \
24 return err; \
25 }
26
27static void *_fdt_grab_space(void *fdt, size_t len)
28{
29 int offset = fdt_size_dt_struct(fdt);
30 int spaceleft;
31
32 spaceleft = fdt_totalsize(fdt) - fdt_off_dt_struct(fdt)
33 - fdt_size_dt_strings(fdt);
34
35 if ((offset + len < offset) || (offset + len > spaceleft))
36 return NULL;
37
38 fdt_set_size_dt_struct(fdt, offset + len);
39 return _fdt_offset_ptr_w(fdt, offset);
40}
41
42int fdt_create(void *buf, int bufsize)
43{
44 void *fdt = buf;
45
46 if (bufsize < sizeof(struct fdt_header))
47 return -FDT_ERR_NOSPACE;
48
49 memset(buf, 0, bufsize);
50
51 fdt_set_magic(fdt, FDT_SW_MAGIC);
52 fdt_set_version(fdt, FDT_LAST_SUPPORTED_VERSION);
53 fdt_set_last_comp_version(fdt, FDT_FIRST_SUPPORTED_VERSION);
54 fdt_set_totalsize(fdt, bufsize);
55
56 fdt_set_off_mem_rsvmap(fdt, FDT_ALIGN(sizeof(struct fdt_header),
57 sizeof(struct fdt_reserve_entry)));
58 fdt_set_off_dt_struct(fdt, fdt_off_mem_rsvmap(fdt));
59 fdt_set_off_dt_strings(fdt, bufsize);
60
61 return 0;
62}
63
64int fdt_resize(void *fdt, void *buf, int bufsize)
65{
66 size_t headsize, tailsize;
67 char *oldtail, *newtail;
68
69 FDT_SW_CHECK_HEADER(fdt);
70
71 headsize = fdt_off_dt_struct(fdt);
72 tailsize = fdt_size_dt_strings(fdt);
73
74 if ((headsize + tailsize) > bufsize)
75 return -FDT_ERR_NOSPACE;
76
77 oldtail = (char *)fdt + fdt_totalsize(fdt) - tailsize;
78 newtail = (char *)buf + bufsize - tailsize;
79
80
81
82 if (buf <= fdt) {
83 memmove(buf, fdt, headsize);
84 memmove(newtail, oldtail, tailsize);
85 } else {
86 memmove(newtail, oldtail, tailsize);
87 memmove(buf, fdt, headsize);
88 }
89
90 fdt_set_off_dt_strings(buf, bufsize);
91 fdt_set_totalsize(buf, bufsize);
92
93 return 0;
94}
95
96int fdt_add_reservemap_entry(void *fdt, uint64_t addr, uint64_t size)
97{
98 struct fdt_reserve_entry *re;
99 int offset;
100
101 FDT_SW_CHECK_HEADER(fdt);
102
103 if (fdt_size_dt_struct(fdt))
104 return -FDT_ERR_BADSTATE;
105
106 offset = fdt_off_dt_struct(fdt);
107 if ((offset + sizeof(*re)) > fdt_totalsize(fdt))
108 return -FDT_ERR_NOSPACE;
109
110 re = (struct fdt_reserve_entry *)((char *)fdt + offset);
111 re->address = cpu_to_fdt64(addr);
112 re->size = cpu_to_fdt64(size);
113
114 fdt_set_off_dt_struct(fdt, offset + sizeof(*re));
115
116 return 0;
117}
118
119int fdt_finish_reservemap(void *fdt)
120{
121 return fdt_add_reservemap_entry(fdt, 0, 0);
122}
123
124int fdt_begin_node(void *fdt, const char *name)
125{
126 struct fdt_node_header *nh;
127 int namelen = strlen(name) + 1;
128
129 FDT_SW_CHECK_HEADER(fdt);
130
131 nh = _fdt_grab_space(fdt, sizeof(*nh) + FDT_TAGALIGN(namelen));
132 if (! nh)
133 return -FDT_ERR_NOSPACE;
134
135 nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE);
136 memcpy(nh->name, name, namelen);
137 return 0;
138}
139
140int fdt_end_node(void *fdt)
141{
142 fdt32_t *en;
143
144 FDT_SW_CHECK_HEADER(fdt);
145
146 en = _fdt_grab_space(fdt, FDT_TAGSIZE);
147 if (! en)
148 return -FDT_ERR_NOSPACE;
149
150 *en = cpu_to_fdt32(FDT_END_NODE);
151 return 0;
152}
153
154static int _fdt_find_add_string(void *fdt, const char *s)
155{
156 char *strtab = (char *)fdt + fdt_totalsize(fdt);
157 const char *p;
158 int strtabsize = fdt_size_dt_strings(fdt);
159 int len = strlen(s) + 1;
160 int struct_top, offset;
161
162 p = _fdt_find_string(strtab - strtabsize, strtabsize, s);
163 if (p)
164 return p - strtab;
165
166
167 offset = -strtabsize - len;
168 struct_top = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
169 if (fdt_totalsize(fdt) + offset < struct_top)
170 return 0;
171
172 memcpy(strtab + offset, s, len);
173 fdt_set_size_dt_strings(fdt, strtabsize + len);
174 return offset;
175}
176
177int fdt_property_placeholder(void *fdt, const char *name, int len, void **valp)
178{
179 struct fdt_property *prop;
180 int nameoff;
181
182 FDT_SW_CHECK_HEADER(fdt);
183
184 nameoff = _fdt_find_add_string(fdt, name);
185 if (nameoff == 0)
186 return -FDT_ERR_NOSPACE;
187
188 prop = _fdt_grab_space(fdt, sizeof(*prop) + FDT_TAGALIGN(len));
189 if (! prop)
190 return -FDT_ERR_NOSPACE;
191
192 prop->tag = cpu_to_fdt32(FDT_PROP);
193 prop->nameoff = cpu_to_fdt32(nameoff);
194 prop->len = cpu_to_fdt32(len);
195 *valp = prop->data;
196 return 0;
197}
198
199int fdt_property(void *fdt, const char *name, const void *val, int len)
200{
201 void *ptr;
202 int ret;
203
204 ret = fdt_property_placeholder(fdt, name, len, &ptr);
205 if (ret)
206 return ret;
207 memcpy(ptr, val, len);
208 return 0;
209}
210
211int fdt_finish(void *fdt)
212{
213 char *p = (char *)fdt;
214 fdt32_t *end;
215 int oldstroffset, newstroffset;
216 uint32_t tag;
217 int offset, nextoffset;
218
219 FDT_SW_CHECK_HEADER(fdt);
220
221
222 end = _fdt_grab_space(fdt, sizeof(*end));
223 if (! end)
224 return -FDT_ERR_NOSPACE;
225 *end = cpu_to_fdt32(FDT_END);
226
227
228 oldstroffset = fdt_totalsize(fdt) - fdt_size_dt_strings(fdt);
229 newstroffset = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
230 memmove(p + newstroffset, p + oldstroffset, fdt_size_dt_strings(fdt));
231 fdt_set_off_dt_strings(fdt, newstroffset);
232
233
234 offset = 0;
235 while ((tag = fdt_next_tag(fdt, offset, &nextoffset)) != FDT_END) {
236 if (tag == FDT_PROP) {
237 struct fdt_property *prop =
238 _fdt_offset_ptr_w(fdt, offset);
239 int nameoff;
240
241 nameoff = fdt32_to_cpu(prop->nameoff);
242 nameoff += fdt_size_dt_strings(fdt);
243 prop->nameoff = cpu_to_fdt32(nameoff);
244 }
245 offset = nextoffset;
246 }
247 if (nextoffset < 0)
248 return nextoffset;
249
250
251 fdt_set_totalsize(fdt, newstroffset + fdt_size_dt_strings(fdt));
252 fdt_set_magic(fdt, FDT_MAGIC);
253 return 0;
254}
255