1// SPDX-License-Identifier: GPL-2.0+
2/*
3 * Check RISC-V boot hart ID
4 *
5 * Copyright 2022, Heinrich Schuchardt <xypron.glpk@gmx.de>
6 *
7 * This test program reads the boot HART ID both from the device-tree from the
8 * RISCV_EFI_BOOT_PROTOCOL and writes both values to the console.
9 */
10
11#include <efi_api.h>
12#include <efi_riscv.h>
13#include <linux/libfdt.h>
14
15static const efi_guid_t riscv_efi_boot_protocol_guid =
16		RISCV_EFI_BOOT_PROTOCOL_GUID;
17static const efi_guid_t fdt_guid = EFI_FDT_GUID;
18
19static struct efi_system_table *systable;
20static struct efi_boot_services *boottime;
21static struct efi_simple_text_output_protocol *con_out;
22static const char *fdt;
23
24/**
25 * Print an unsigned 32bit value as decimal number to an u16 string
26 *
27 * @value:	value to be printed
28 * @buf:	pointer to buffer address
29 */
30static void uint2dec(u32 value, u16 *buf)
31{
32	u16 *pos = buf;
33	int i;
34	u16 c;
35	u64 f;
36
37	/*
38	 * Increment by .5 and multiply with
39	 * (2 << 60) / 1,000,000,000 = 0x44B82FA0.9B5A52CC
40	 * to move the first digit to bit 60-63.
41	 */
42	f = 0x225C17D0;
43	f += (0x9B5A52DULL * value) >> 28;
44	f += 0x44B82FA0ULL * value;
45
46	for (i = 0; i < 10; ++i) {
47		/* Write current digit */
48		c = f >> 60;
49		if (c || pos != buf)
50			*pos++ = c + '0';
51		/* Eliminate current digit */
52		f &= 0xfffffffffffffff;
53		/* Get next digit */
54		f *= 0xaULL;
55	}
56	if (pos == buf)
57		*pos++ = '0';
58	*pos = 0;
59}
60
61/**
62 * f2h() - convert FDT value to host endianness.
63 *
64 * UEFI code is always low endian. The FDT is big endian.
65 *
66 * @val:	FDT value
67 * Return:	converted value
68 */
69static uint32_t f2h(fdt32_t val)
70{
71	char *buf = (char *)&val;
72	char i;
73
74	/* Swap the bytes */
75	i = buf[0]; buf[0] = buf[3]; buf[3] = i;
76	i = buf[1]; buf[1] = buf[2]; buf[2] = i;
77
78	return val;
79}
80
81/**
82 * memcomp() - compare two memory buffers
83 *
84 * s1:		first buffer
85 * s2:		second buffer
86 * n:		size of buffers
87 * Return:	0 if both buffers have the same content
88 */
89static int memcomp(const void *s1, const void *s2, size_t n)
90{
91	const char *pos1 = s1, *pos2 = s2;
92
93	for (size_t count = 0; count < n ; ++pos1, ++pos2, --count) {
94		if (*pos1 != *pos2)
95			return *pos1 - *pos2;
96	}
97	return 0;
98}
99
100/**
101 * strcomp() - compare to strings
102 *
103 * @buf1:	first string
104 * @buf2:	second string
105 * Return:	0 if both strings are the same
106 */
107static int strcomp(const char *buf1, const char *buf2)
108{
109	for (; *buf1 || *buf2; ++buf1, ++buf2) {
110		if (*buf1 != *buf2)
111			return *buf1 - *buf2;
112	}
113	return 0;
114}
115
116/**
117 * get_property() - return value of a property of an FDT node
118 *
119 * A property of the root node or one of its direct children can be
120 * retrieved.
121 *
122 * @property	name of the property
123 * @node	name of the node or NULL for root node
124 * Return:	value of the property
125 */
126static char *get_property(const char *property, const char *node)
127{
128	struct fdt_header *header = (struct fdt_header *)fdt;
129	const fdt32_t *end;
130	const fdt32_t *pos;
131	const char *strings;
132	size_t level = 0;
133	const char *nodelabel = NULL;
134
135	if (!header) {
136		con_out->output_string(con_out, u"Missing device tree\r\n");
137		return NULL;
138	}
139
140	if (f2h(header->magic) != FDT_MAGIC) {
141		con_out->output_string(con_out, u"Wrong device tree magic\r\n");
142		return NULL;
143	}
144
145	pos = (fdt32_t *)(fdt + f2h(header->off_dt_struct));
146	end = &pos[f2h(header->totalsize) >> 2];
147	strings = fdt + f2h(header->off_dt_strings);
148
149	for (; pos < end;) {
150		switch (f2h(pos[0])) {
151		case FDT_BEGIN_NODE: {
152			const char *c = (char *)&pos[1];
153			size_t i;
154
155			if (level == 1)
156				nodelabel = c;
157			++level;
158			for (i = 0; c[i]; ++i)
159				;
160			pos = &pos[2 + (i >> 2)];
161			break;
162		}
163		case FDT_PROP: {
164			struct fdt_property *prop = (struct fdt_property *)pos;
165			const char *label = &strings[f2h(prop->nameoff)];
166			efi_status_t ret;
167
168			/* Check if this is the property to be returned */
169			if (!strcomp(property, label) &&
170			    ((level == 1 && !node) ||
171			     (level == 2 && node &&
172			      !strcomp(node, nodelabel)))) {
173				char *str;
174				efi_uintn_t len = f2h(prop->len);
175
176				if (!len)
177					return NULL;
178				/*
179				 * The string might not be 0 terminated.
180				 * It is safer to make a copy.
181				 */
182				ret = boottime->allocate_pool(
183					EFI_LOADER_DATA, len + 1,
184					(void **)&str);
185				if (ret != EFI_SUCCESS) {
186					con_out->output_string(
187						    con_out,
188						    u"AllocatePool failed\r\n");
189					return NULL;
190				}
191				boottime->copy_mem(str, &pos[3], len);
192				str[len] = 0;
193
194				return str;
195			}
196
197			pos = &pos[3 + ((f2h(prop->len) + 3) >> 2)];
198			break;
199		}
200		case FDT_NOP:
201			++pos;
202			break;
203		case FDT_END_NODE:
204			--level;
205			++pos;
206			break;
207		case FDT_END:
208			return NULL;
209		default:
210			con_out->output_string(
211				con_out, u"Invalid device tree token\r\n");
212			return NULL;
213		}
214	}
215	con_out->output_string(
216		con_out, u"Missing FDT_END token\r\n");
217	return NULL;
218}
219
220/**
221 * get_config_table() - get configuration table
222 *
223 * @guid:	table GUID
224 * Return:	pointer to table or NULL
225 */
226static void *get_config_table(const efi_guid_t *guid)
227{
228	size_t i;
229
230	for (i = 0; i < systable->nr_tables; i++) {
231		if (!memcomp(guid, &systable->tables[i].guid, 16))
232			return systable->tables[i].table;
233	}
234	return NULL;
235}
236
237/**
238 * fdt_get_hart() - get hart ID via RISC-V device-tree
239 *
240 * @hartid:	boot hart ID
241 * Return:	status code
242 */
243static efi_status_t fdt_get_hart(efi_uintn_t *hartid)
244{
245	char *str;
246
247	fdt = get_config_table(&fdt_guid);
248	if (!fdt) {
249		con_out->output_string(con_out, u"Missing device tree\r\n");
250		return EFI_NOT_FOUND;
251	}
252
253	str = get_property("boot-hartid", "chosen");
254	if (!str) {
255		con_out->output_string(con_out,
256				       u"/chosen/boot-hartid missing\r\n");
257		return EFI_NOT_FOUND;
258	}
259	*hartid = f2h(*(fdt32_t *)str);
260	boottime->free_pool(str);
261
262	return EFI_SUCCESS;
263}
264
265/**
266 * prot_get_hart() - get hart ID via RISC-V Boot Protocol
267 *
268 * @hartid:	boot hart ID
269 * Return:	status code
270 */
271static efi_status_t prot_get_hart(efi_uintn_t *hartid)
272{
273	efi_status_t ret;
274	struct riscv_efi_boot_protocol *prot;
275
276	/* Get RISC-V boot protocol */
277	ret = boottime->locate_protocol(&riscv_efi_boot_protocol_guid, NULL,
278					(void **)&prot);
279	if (ret != EFI_SUCCESS) {
280		con_out->output_string(
281			con_out, u"RISC-V Boot Protocol not available\r\n");
282		return ret;
283	}
284
285	/* Get boot hart ID from EFI protocol */
286	ret = prot->get_boot_hartid(prot, hartid);
287	if (ret != EFI_SUCCESS)
288		con_out->output_string(con_out,
289				       u"Could not retrieve boot hart ID\r\n");
290	return ret;
291}
292
293/**
294 * efi_main() - entry point of the EFI application.
295 *
296 * @handle:	handle of the loaded image
297 * @systab:	system table
298 * Return:	status code
299 */
300efi_status_t EFIAPI efi_main(efi_handle_t handle,
301			     struct efi_system_table *systab)
302{
303	efi_status_t ret;
304	efi_uintn_t hartid;
305	u16 buf[16];
306
307	systable = systab;
308	boottime = systable->boottime;
309	con_out = systable->con_out;
310
311	con_out->output_string(con_out,
312			       u"\r\nBoot hart ID\r\n------------\r\n\r\n");
313
314	ret = fdt_get_hart(&hartid);
315	if (ret == EFI_SUCCESS) {
316		con_out->output_string(con_out, u"Device-tree: ");
317		uint2dec(hartid, buf);
318		con_out->output_string(con_out, buf);
319		con_out->output_string(con_out, u"\r\n");
320	}
321
322	ret = prot_get_hart(&hartid);
323	if (ret == EFI_SUCCESS) {
324		con_out->output_string(con_out, u"RISCV_EFI_BOOT_PROTOCOL: ");
325		uint2dec(hartid, buf);
326		con_out->output_string(con_out, buf);
327		con_out->output_string(con_out, u"\r\n");
328	}
329
330	con_out->output_string(con_out, u"\r\n");
331	boottime->exit(handle, EFI_SUCCESS, 0, NULL);
332
333	/* We should never arrive here */
334	return EFI_SUCCESS;
335}
336