diff --git a/common/fdt_wrappers.c b/common/fdt_wrappers.c new file mode 100644 index 000000000..376601940 --- /dev/null +++ b/common/fdt_wrappers.c @@ -0,0 +1,96 @@ +/* + * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +/* Helper functions to offer easier navigation of Device Tree Blob */ + +#include +#include +#include +#include + +/* + * Read cells from a given property of the given node. At most 2 cells of the + * property are read, and pointer is updated. Returns 0 on success, and -1 upon + * error + */ +int fdtw_read_cells(const void *dtb, int node, const char *prop, + unsigned int cells, void *value) +{ + const uint32_t *value_ptr; + uint32_t hi = 0, lo; + int value_len; + + assert(dtb); + assert(prop); + assert(value); + assert(node >= 0); + + /* We expect either 1 or 2 cell property */ + assert(cells <= 2); + + /* Access property and obtain its length (in bytes) */ + value_ptr = fdt_getprop_namelen(dtb, node, prop, strlen(prop), + &value_len); + if (value_ptr == NULL) { + WARN("Couldn't find property %s in dtb\n", prop); + return -1; + } + + + /* Verify that property length accords with cell length */ + if (NCELLS(value_len) != cells) { + WARN("Property length mismatch\n"); + return -1; + } + + if (cells == 2) { + hi = fdt32_to_cpu(*value_ptr); + value_ptr++; + } + + lo = fdt32_to_cpu(*value_ptr); + + if (cells == 2) + *((uint64_t *) value) = ((uint64_t) hi << 32) | lo; + else + *((uint32_t *) value) = lo; + + return 0; +} + +/* + * Write cells in place to a given property of the given node. At most 2 cells + * of the property are written. Returns 0 on success, and -1 upon error. + */ +int fdtw_write_inplace_cells(void *dtb, int node, const char *prop, + unsigned int cells, void *value) +{ + int err, len; + + assert(dtb); + assert(prop); + assert(value); + assert(node >= 0); + + /* We expect either 1 or 2 cell property */ + assert(cells <= 2); + + if (cells == 2) + *(uint64_t *)value = cpu_to_fdt64(*(uint64_t *)value); + else + *(uint32_t *)value = cpu_to_fdt32(*(uint32_t *)value); + + len = cells * 4; + + /* Set property value in place */ + err = fdt_setprop_inplace(dtb, node, prop, value, len); + if (err != 0) { + WARN("Modify property %s failed with error %d\n", prop, err); + return -1; + } + + return 0; +} diff --git a/include/common/fdt_wrappers.h b/include/common/fdt_wrappers.h new file mode 100644 index 000000000..db72f1803 --- /dev/null +++ b/include/common/fdt_wrappers.h @@ -0,0 +1,19 @@ +/* + * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +/* Helper functions to offer easier navigation of Device Tree Blob */ + +#ifndef __FDT_WRAPPERS__ +#define __FDT_WRAPPERS__ + +/* Number of cells, given total length in bytes. Each cell is 4 bytes long */ +#define NCELLS(l) (l / 4) + +int fdtw_read_cells(const void *dtb, int node, const char *prop, + unsigned int cells, void *value); +int fdtw_write_inplace_cells(void *dtb, int node, const char *prop, + unsigned int cells, void *value); +#endif /* __FDT_WRAPPERS__ */