diff options
Diffstat (limited to 'components/drivers/ofw/raw.c')
| -rwxr-xr-x | components/drivers/ofw/raw.c | 197 |
1 files changed, 197 insertions, 0 deletions
diff --git a/components/drivers/ofw/raw.c b/components/drivers/ofw/raw.c new file mode 100755 index 0000000000..35e7aaa343 --- /dev/null +++ b/components/drivers/ofw/raw.c @@ -0,0 +1,197 @@ +/* + * Copyright (c) 2006-2022, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + * 2022-10-19 GuEe-GUI first version + */ + +#include <drivers/ofw_raw.h> + +int fdt_add_subnode_possible(void *fdt, int parentoffset, const char *name) +{ + int nodeoffset; + + if ((nodeoffset = fdt_add_subnode(fdt, parentoffset, name)) < 0) + { + fdt_open_into(fdt, fdt, fdt_totalsize(fdt) + FDT_PADDING_SIZE); + nodeoffset = fdt_add_subnode(fdt, parentoffset, name); + } + + return nodeoffset; +} + +int fdt_add_mem_rsv_possible(void *fdt, size_t addr, size_t size) +{ + int err = 0; + + if (fdt_add_mem_rsv(fdt, addr, size) < 0) + { + fdt_open_into(fdt, fdt, fdt_totalsize(fdt) + FDT_PADDING_SIZE); + err = fdt_add_mem_rsv(fdt, addr, size); + } + + return err; +} + +int fdt_setprop_uxx(void *fdt, int nodeoffset, const char *name, uint64_t val, bool is_u64) +{ + int err; + + if (is_u64) + { + err = fdt_setprop_u64(fdt, nodeoffset, name, val); + } + else + { + err = fdt_setprop_u32(fdt, nodeoffset, name, (uint32_t)val); + } + + return err; +} + +#define FDT_RAW_GET_VAL_FLAG(std_type, s, sz) \ +int fdt_getprop_##std_type##sz(void *fdt, int nodeoffset, \ + const char *name, s##int##sz##_t *out_value, int *lenp) \ +{ \ + int err = -FDT_ERR_NOTFOUND; \ + if (fdt && nodeoffset >= 0 && name && out_value) \ + { \ + const fdt##sz##_t *ptr; \ + if ((ptr = fdt_getprop(fdt, nodeoffset, name, lenp))) \ + { \ + *out_value = fdt##sz##_to_cpu(*ptr); \ + err = 0; \ + } \ + } \ + return err; \ +} + +#define FDT_RAW_GET_VAL(size) \ + FDT_RAW_GET_VAL_FLAG(u, u, size) \ + FDT_RAW_GET_VAL_FLAG(s, , size) + +FDT_RAW_GET_VAL(64) +FDT_RAW_GET_VAL(32) +FDT_RAW_GET_VAL(16) +FDT_RAW_GET_VAL(8) + +#undef FDT_RAW_GET_VAL +#undef FDT_RAW_GET_VAL_FLAG + +int fdt_io_addr_cells(void *fdt, int nodeoffset) +{ + int cells = -1; + int parentoffset = fdt_parent_offset(fdt, nodeoffset); + + for (; parentoffset >= 0 ; parentoffset = fdt_parent_offset(fdt, parentoffset)) + { + const fdt32_t *cells_tmp = fdt_getprop(fdt, parentoffset, "#address-cells", NULL); + + if (cells_tmp) + { + cells = fdt32_to_cpu(*cells_tmp); + } + } + + if (cells < 0) + { + cells = fdt_address_cells(fdt, nodeoffset); + } + + return cells; +} + +int fdt_io_size_cells(void *fdt, int nodeoffset) +{ + int cells = -1; + int parentoffset = fdt_parent_offset(fdt, nodeoffset); + + for (; parentoffset >= 0 ; parentoffset = fdt_parent_offset(fdt, parentoffset)) + { + const fdt32_t *cells_tmp = fdt_getprop(fdt, parentoffset, "#size-cells", NULL); + + if (cells_tmp) + { + cells = fdt32_to_cpu(*cells_tmp); + } + } + + if (cells < 0) + { + cells = fdt_size_cells(fdt, nodeoffset); + } + + return cells; +} + +int fdt_install_initrd(void *fdt, char *os_name, size_t initrd_addr, size_t initrd_size) +{ + int err = -FDT_ERR_NOTFOUND; + int chosen_offset = -1, root_off = fdt_path_offset(fdt, "/"); + + if (root_off >= 0) + { + chosen_offset = fdt_subnode_offset(fdt, root_off, "chosen"); + + if (chosen_offset == -FDT_ERR_NOTFOUND) + { + chosen_offset = fdt_add_subnode_possible(fdt, root_off, "chosen"); + } + } + + if (chosen_offset >= 0) + { + uint64_t addr, size; + + err = 0; + + /* Update the entry */ + for (int i = fdt_num_mem_rsv(fdt) - 1; i >= 0; --i) + { + fdt_get_mem_rsv(fdt, i, &addr, &size); + + if (addr == initrd_addr) + { + fdt_del_mem_rsv(fdt, i); + break; + } + } + + /* Add the memory */ + if (fdt_add_mem_rsv(fdt, initrd_addr, initrd_size) < 0) + { + /* Move the memory */ + fdt_open_into(fdt, fdt, fdt_totalsize(fdt) + FDT_PADDING_SIZE); + + if (fdt_add_mem_rsv(fdt, initrd_addr, initrd_size) < 0) + { + err = -FDT_ERR_NOSPACE; + } + } + + if (!err) + { + size_t name_len; + char initrd_name[64]; + bool is_u64 = (fdt_io_addr_cells(fdt, root_off) == 2); + + if (!os_name) + { + os_name = "rt-thread"; + } + + name_len = strlen(initrd_name); + + strncpy(&initrd_name[name_len], ",initrd-start", sizeof(initrd_name) - name_len); + fdt_setprop_uxx(fdt, chosen_offset, initrd_name, initrd_addr, is_u64); + + strncpy(&initrd_name[name_len], ",initrd-end", sizeof(initrd_name) - name_len); + fdt_setprop_uxx(fdt, chosen_offset, initrd_name, initrd_addr + initrd_size, is_u64); + } + } + + return err; +} |