static u32 libfdt_property_len(const char *prop, u32 address_cells, u32 size_cells, u32 len) { u32 lsz, type, reg_cells, reg_count; /* Special way of handling 'reg' property */ if (strcmp(prop, "reg") == 0) { reg_cells = len / sizeof(fdt_cell_t); reg_count = udiv32(reg_cells, address_cells + size_cells); if (umod32(reg_cells, address_cells + size_cells)) { reg_count++; } reg_cells = sizeof(physical_addr_t) / sizeof(fdt_cell_t); reg_cells += sizeof(physical_size_t) / sizeof(fdt_cell_t); reg_count = reg_count * (reg_cells * sizeof(fdt_cell_t)); return reg_count; } type = vmm_devtree_estimate_attrtype(prop); /* Special way of handling non-literal property */ if (!vmm_devtree_isliteral(type)) { return len; } /* Special way of handling literal property */ lsz = vmm_devtree_literal_size(type); if (umod32(len, lsz)) { return udiv32(len, lsz) * lsz + lsz; } return len; }
static void libfdt_parse_devtree_recursive(struct fdt_fileinfo * fdt, struct vmm_devtree_node * node, char **data) { u32 type, len; const char * name; struct vmm_devtree_node *child; if (!fdt || !node) { return; } while (LIBFDT_DATA32(*data) != FDT_END_NODE) { switch (LIBFDT_DATA32(*data)) { case FDT_PROP: *data += sizeof(fdt_cell_t); len = LIBFDT_DATA32(*data); *data += sizeof(fdt_cell_t); name = &fdt->str[LIBFDT_DATA32(*data)]; *data += sizeof(fdt_cell_t); type = vmm_devtree_estimate_attrtype(name); vmm_devtree_setattr(node, name, *data, type, len); *data += len; while ((virtual_addr_t) (*data) % sizeof(fdt_cell_t) != 0) (*data)++; break; case FDT_NOP: *data += sizeof(fdt_cell_t); break; case FDT_BEGIN_NODE: *data += sizeof(fdt_cell_t); type = VMM_DEVTREE_NODETYPE_UNKNOWN; child = vmm_devtree_addnode(node, *data, type, NULL); *data += vmm_strlen(*data) + 1; while ((virtual_addr_t) (*data) % sizeof(fdt_cell_t) != 0) { (*data)++; } libfdt_parse_devtree_recursive(fdt, child, data); break; default: return; break; }; } *data += sizeof(fdt_cell_t); return; }
static void libfdt_property_read(const char *prop, void *dst, void *src, u32 address_cells, u32 size_cells, u32 len) { int tlen; u32 pos, type, lsz, val32, reg, reg_cells, reg_count; u64 val64; /* Special way of handling 'reg' property */ if (strcmp(prop, "reg") == 0) { reg_cells = len / sizeof(fdt_cell_t); reg_count = udiv32(reg_cells, address_cells + size_cells); if (umod32(reg_cells, address_cells + size_cells)) { reg_count++; } for (reg = 0; reg < reg_count; reg++) { if (address_cells == 2) { *((physical_addr_t *)dst) = (physical_addr_t)LIBFDT_DATA64(src); } else { *((physical_addr_t *)dst) = (physical_addr_t)LIBFDT_DATA32(src); } dst += sizeof(physical_addr_t); src += address_cells * sizeof(fdt_cell_t); if (size_cells == 2) { *((physical_size_t *)dst) = (physical_size_t)LIBFDT_DATA64(src); } else { *((physical_size_t *)dst) = (physical_size_t)LIBFDT_DATA32(src); } dst += sizeof(physical_size_t); src += size_cells * sizeof(fdt_cell_t); } return; } type = vmm_devtree_estimate_attrtype(prop); /* Special way of handling non-literal property */ if (!vmm_devtree_isliteral(type)) { memcpy(dst, src, len); return; } /* Special way of handling literal property */ lsz = vmm_devtree_literal_size(type); if (lsz == 4) { pos = 0; tlen = len; while (tlen > 0) { if (tlen < 4) { break; } val32 = ((u32 *)src)[pos]; ((u32 *)dst)[pos] = LIBFDT_DATA32(&val32); pos++; tlen -= 4; } } else if (lsz == 8) { pos = 0; tlen = len; while (tlen > 0) { if (tlen < 4) { break; } else if (tlen == 4) { ((u64 *)dst)[pos] = LIBFDT_DATA32(&((u32 *)src)[pos*2]); break; } val64 = ((u64 *)src)[pos]; ((u64 *)dst)[pos] = LIBFDT_DATA64(&val64); pos++; tlen -= 8; } } }
static void libfdt_parse_devtree_recursive(struct fdt_fileinfo *fdt, struct vmm_devtree_node *node, char **data) { void *val; const char *name; u32 type, len, alen, addr_cells, size_cells; struct vmm_devtree_node *child; if (!fdt || !node) { return; } if (vmm_devtree_read_u32(node->parent, "#address-cells", &addr_cells)) { addr_cells = sizeof(physical_addr_t) / sizeof(fdt_cell_t); } if (vmm_devtree_read_u32(node->parent, "#size-cells", &size_cells)) { size_cells = sizeof(physical_size_t) / sizeof(fdt_cell_t); } while (LIBFDT_DATA32(*data) != FDT_END_NODE) { switch (LIBFDT_DATA32(*data)) { case FDT_PROP: *data += sizeof(fdt_cell_t); len = LIBFDT_DATA32(*data); *data += sizeof(fdt_cell_t); name = &fdt->str[LIBFDT_DATA32(*data)]; *data += sizeof(fdt_cell_t); type = vmm_devtree_estimate_attrtype(name); alen = libfdt_property_len(name, addr_cells, size_cells, len); val = vmm_zalloc(alen); libfdt_property_read(name, val, *data, addr_cells, size_cells, len); vmm_devtree_setattr(node, name, val, type, alen); vmm_free(val); *data += len; while ((virtual_addr_t) (*data) % sizeof(fdt_cell_t) != 0) (*data)++; break; case FDT_NOP: *data += sizeof(fdt_cell_t); break; case FDT_BEGIN_NODE: *data += sizeof(fdt_cell_t); child = vmm_devtree_addnode(node, *data); *data += strlen(*data) + 1; while ((virtual_addr_t) (*data) % sizeof(fdt_cell_t) != 0) { (*data)++; } libfdt_parse_devtree_recursive(fdt, child, data); break; default: return; break; }; } *data += sizeof(fdt_cell_t); return; }