int fdt_get_resource(const void *fdt, int node, const char *property,
unsigned int index, struct fdt_resource *res)
{
const fdt32_t *ptr, *end;
int na, ns, len, parent;
unsigned int i = 0;
parent = fdt_parent_offset(fdt, node);
if (parent < 0)
return parent;
na = fdt_address_cells(fdt, parent);
ns = fdt_size_cells(fdt, parent);
ptr = fdt_getprop(fdt, node, property, &len);
if (!ptr)
return len;
end = ptr + len / sizeof(*ptr);
while (ptr + na + ns <= end) {
if (i == index) {
res->start = res->end = fdtdec_get_number(ptr, na);
res->end += fdtdec_get_number(&ptr[na], ns) - 1;
return 0;
}
ptr += na + ns;
i++;
}
return -FDT_ERR_NOTFOUND;
}
fdt_addr_t fdtdec_get_addr_size_auto_parent(const void *blob, int parent,
int node, const char *prop_name, int index, fdt_size_t *sizep,
bool translate)
{
int na, ns;
debug("%s: ", __func__);
na = fdt_address_cells(blob, parent);
if (na < 1) {
debug("(bad #address-cells)\n");
return FDT_ADDR_T_NONE;
}
ns = fdt_size_cells(blob, parent);
if (ns < 0) {
debug("(bad #size-cells)\n");
return FDT_ADDR_T_NONE;
}
debug("na=%d, ns=%d, ", na, ns);
return fdtdec_get_addr_size_fixed(blob, node, prop_name, index, na,
ns, sizep, translate);
}
fdt_addr_t fdtdec_get_addr_size(const void *blob, int node,
const char *prop_name, fdt_size_t *sizep)
{
const fdt32_t *ptr, *end;
int parent, na, ns, len;
fdt_addr_t addr;
debug("%s: %s: ", __func__, prop_name);
parent = fdt_parent_offset(blob, node);
if (parent < 0) {
debug("(no parent found)\n");
return FDT_ADDR_T_NONE;
}
na = fdt_address_cells(blob, parent);
ns = fdt_size_cells(blob, parent);
ptr = fdt_getprop(blob, node, prop_name, &len);
if (!ptr) {
debug("(not found)\n");
return FDT_ADDR_T_NONE;
}
end = ptr + len / sizeof(*ptr);
if (ptr + na + ns > end) {
debug("(not enough data: expected %d bytes, got %d bytes)\n",
(na + ns) * 4, len);
return FDT_ADDR_T_NONE;
}
addr = fdtdec_get_number(ptr, na);
if (sizep) {
*sizep = fdtdec_get_number(ptr + na, ns);
debug("addr=%pa, size=%pa\n", &addr, sizep);
} else {
debug("%pa\n", &addr);
}
return addr;
}
/*
* fdt_pack_reg - pack address and size array into the "reg"-suitable stream
*/
static int fdt_pack_reg(const void *fdt, void *buf, u64 *address, u64 *size,
int n)
{
int i;
int address_cells = fdt_address_cells(fdt, 0);
int size_cells = fdt_size_cells(fdt, 0);
char *p = buf;
for (i = 0; i < n; i++) {
if (address_cells == 2)
*(fdt64_t *)p = cpu_to_fdt64(address[i]);
else
*(fdt32_t *)p = cpu_to_fdt32(address[i]);
p += 4 * address_cells;
if (size_cells == 2)
*(fdt64_t *)p = cpu_to_fdt64(size[i]);
else
*(fdt32_t *)p = cpu_to_fdt32(size[i]);
p += 4 * size_cells;
}
return p - (char *)buf;
}
ssize_t _fdt_reg_size(const void *fdt, int offs)
{
const uint32_t *reg;
uint32_t sz;
int n;
int len;
int parent;
parent = fdt_parent_offset(fdt, offs);
if (parent < 0)
return (paddr_t)-1;
reg = (const uint32_t *)fdt_getprop(fdt, offs, "reg", &len);
if (!reg)
return -1;
n = fdt_address_cells(fdt, parent);
if (n < 1 || n > 2)
return -1;
reg += n;
n = fdt_size_cells(fdt, parent);
if (n < 1 || n > 2)
return -1;
sz = fdt32_to_cpu(*reg);
if (n == 2) {
if (sz)
return -1;
reg++;
sz = fdt32_to_cpu(*reg);
}
return sz;
}
/*
* fdt_get_reg - Fill buffer by information from DT
*/
static phys_size_t fdt_get_reg(const void *fdt, int nodeoffset, void *buf,
const u32 *cell, int n)
{
int i = 0, b, banks;
int parent_offset = fdt_parent_offset(fdt, nodeoffset);
int address_cells = fdt_address_cells(fdt, parent_offset);
int size_cells = fdt_size_cells(fdt, parent_offset);
char *p = buf;
phys_addr_t val;
phys_size_t vals;
debug("%s: addr_cells=%x, size_cell=%x, buf=%p, cell=%p\n",
__func__, address_cells, size_cells, buf, cell);
/* Check memory bank setup */
banks = n % (address_cells + size_cells);
if (banks)
panic("Incorrect memory setup cells=%d, ac=%d, sc=%d\n",
n, address_cells, size_cells);
banks = n / (address_cells + size_cells);
for (b = 0; b < banks; b++) {
debug("%s: Bank #%d:\n", __func__, b);
if (address_cells == 2) {
val = cell[i + 1];
val <<= 32;
val |= cell[i];
val = fdt64_to_cpu(val);
debug("%s: addr64=%llx, ptr=%p, cell=%p\n",
__func__, val, p, &cell[i]);
*(phys_addr_t *)p = val;
} else {
debug("%s: addr32=%x, ptr=%p\n",
__func__, fdt32_to_cpu(cell[i]), p);
*(phys_addr_t *)p = fdt32_to_cpu(cell[i]);
}
p += sizeof(phys_addr_t);
i += address_cells;
debug("%s: pa=%p, i=%x, size=%zu\n", __func__, p, i,
sizeof(phys_addr_t));
if (size_cells == 2) {
vals = cell[i + 1];
vals <<= 32;
vals |= cell[i];
vals = fdt64_to_cpu(vals);
debug("%s: size64=%llx, ptr=%p, cell=%p\n",
__func__, vals, p, &cell[i]);
*(phys_size_t *)p = vals;
} else {
debug("%s: size32=%x, ptr=%p\n",
__func__, fdt32_to_cpu(cell[i]), p);
*(phys_size_t *)p = fdt32_to_cpu(cell[i]);
}
p += sizeof(phys_size_t);
i += size_cells;
debug("%s: ps=%p, i=%x, size=%zu\n",
__func__, p, i, sizeof(phys_size_t));
}
/* Return the first address size */
return *(phys_size_t *)((char *)buf + sizeof(phys_addr_t));
}
static void load_ranges(const char *dtb)
{
// First iterate through to count the total number of simple-bus
// items
int soc = fdt_node_offset_by_compatible(dtb, -1, "simple-bus");
sr_items = 0;
while(soc != -FDT_ERR_NOTFOUND)
{
int ac = fdt_address_cells(dtb, soc);
int sc = fdt_size_cells(dtb, soc);
int ranges_plen;
const void *ranges = fdt_getprop(dtb, soc, "ranges", &ranges_plen);
if(ranges)
sr_items += ranges_plen / ((2 * ac + sc) * 4);
soc = fdt_node_offset_by_compatible(dtb, soc, "simple-bus");
}
if(sr)
free(sr);
sr = (struct soc_range *)malloc(sr_items * sizeof(struct soc_range));
// Now re-iterate to get the actual values
int cur_sr_item = 0;
soc = fdt_node_offset_by_compatible(dtb, -1, "simple-bus");
while(soc != -FDT_ERR_NOTFOUND)
{
int ac = fdt_address_cells(dtb, soc);
int sc = fdt_size_cells(dtb, soc);
int ranges_plen;
const void *ranges = fdt_getprop(dtb, soc, "ranges", &ranges_plen);
if(ranges)
{
#ifdef DEBUG
printf("DTB: soc %i, ac %i, sc %i, plen %i\n", soc, ac, sc, ranges_plen);
#endif
int num_ranges = ranges_plen / ((2 * ac + sc) * 4);
int offset = 0;
// if more than one 4-byte word is specified for address
// we take the last one only (DTB is big-endian)
for(int i = 0; i < num_ranges; i++, cur_sr_item++)
{
uint32_t cur_child = 0;
uint32_t cur_parent = 0;
uint32_t cur_len = 0;
for(int j = 0; j < ac; j++)
{
cur_child = read_wordbe(ranges, offset);
offset += 4;
}
for(int j = 0; j < ac; j++)
{
cur_parent = read_wordbe(ranges, offset);
offset += 4;
}
for(int j = 0; j < sc; j++)
{
cur_len = read_wordbe(ranges, offset);
offset += 4;
}
#ifdef DEBUG
printf("DTB: ranges: child: %x, parent: %x; len: %x\n",
cur_child, cur_parent, cur_len);
#endif
sr[cur_sr_item].soc_node = soc;
sr[cur_sr_item].child_addr = cur_child;
sr[cur_sr_item].parent_addr = cur_parent;
sr[cur_sr_item].length = cur_len;
}
}
soc = fdt_node_offset_by_compatible(dtb, soc, "simple-bus");
}
}
int fdtdec_decode_ram_size(const void *blob, const char *area, int board_id,
phys_addr_t *basep, phys_size_t *sizep, bd_t *bd)
{
int addr_cells, size_cells;
const u32 *cell, *end;
u64 total_size, size, addr;
int node, child;
bool auto_size;
int bank;
int len;
debug("%s: board_id=%d\n", __func__, board_id);
if (!area)
area = "/memory";
node = fdt_path_offset(blob, area);
if (node < 0) {
debug("No %s node found\n", area);
return -ENOENT;
}
cell = fdt_getprop(blob, node, "reg", &len);
if (!cell) {
debug("No reg property found\n");
return -ENOENT;
}
addr_cells = fdt_address_cells(blob, node);
size_cells = fdt_size_cells(blob, node);
/* Check the board id and mask */
for (child = fdt_first_subnode(blob, node);
child >= 0;
child = fdt_next_subnode(blob, child)) {
int match_mask, match_value;
match_mask = fdtdec_get_int(blob, child, "match-mask", -1);
match_value = fdtdec_get_int(blob, child, "match-value", -1);
if (match_value >= 0 &&
((board_id & match_mask) == match_value)) {
/* Found matching mask */
debug("Found matching mask %d\n", match_mask);
node = child;
cell = fdt_getprop(blob, node, "reg", &len);
if (!cell) {
debug("No memory-banks property found\n");
return -EINVAL;
}
break;
}
}
/* Note: if no matching subnode was found we use the parent node */
if (bd) {
memset(bd->bi_dram, '\0', sizeof(bd->bi_dram[0]) *
CONFIG_NR_DRAM_BANKS);
}
auto_size = fdtdec_get_bool(blob, node, "auto-size");
total_size = 0;
end = cell + len / 4 - addr_cells - size_cells;
debug("cell at %p, end %p\n", cell, end);
for (bank = 0; bank < CONFIG_NR_DRAM_BANKS; bank++) {
if (cell > end)
break;
addr = 0;
if (addr_cells == 2)
addr += (u64)fdt32_to_cpu(*cell++) << 32UL;
addr += fdt32_to_cpu(*cell++);
if (bd)
bd->bi_dram[bank].start = addr;
if (basep && !bank)
*basep = (phys_addr_t)addr;
size = 0;
if (size_cells == 2)
size += (u64)fdt32_to_cpu(*cell++) << 32UL;
size += fdt32_to_cpu(*cell++);
if (auto_size) {
u64 new_size;
debug("Auto-sizing %llx, size %llx: ", addr, size);
new_size = get_ram_size((long *)(uintptr_t)addr, size);
if (new_size == size) {
debug("OK\n");
} else {
debug("sized to %llx\n", new_size);
size = new_size;
}
}
if (bd)
bd->bi_dram[bank].size = size;
total_size += size;
}
debug("Memory size %llu\n", total_size);
if (sizep)
*sizep = (phys_size_t)total_size;
return 0;
}
void dt_platform_init_fdt(void *fdt)
{
int rv;
system_fdt = fdt;
printf("fdt_platform_init %p\n", fdt);
// Check header
if ((rv = fdt_check_header(fdt))) {
panic("Bad FDT: %s\n", fdt_strerror(rv));
}
// Process memory reservations
printf("rsv %p\n", fdt);
int num_rsv = fdt_num_mem_rsv(fdt);
for (int i = 0; i < num_rsv; i++) {
printf("get\n");
gd_memory_map_entry ent;
ent.type = gd_reserved_memory_type;
ent.attributes = 0;
fdt_get_mem_rsv(fdt, i, &ent.physical_start, &ent.size);
printf("got\n");
ent.virtual_start = ent.physical_start;
printf("Adding reserved memory region: %" PRIX64 " len=%" PRIX64 "\n",
ent.physical_start, ent.size);
mmap_add_entry(ent);
}
// Process root note memory entries
printf("root\n");
int root = fdt_next_node(fdt, -1, NULL);
unsigned addr_cells = fdt_address_cells(fdt, root);
unsigned size_cells = fdt_size_cells(fdt, root);
unsigned cells = addr_cells + size_cells;
int mem_offs = fdt_subnode_offset(fdt, root, "memory");
if (mem_offs < 0) {
panic("Unable to locate memory node (%s)\n", fdt_strerror(mem_offs));
}
int memlen;
const uint32_t *p = fdt_getprop(system_fdt, mem_offs, "reg", &memlen);
for (int i = 0; i < memlen / 4; i+= cells) {
uint64_t base_addr = 0, base_size = 0;
if (addr_cells == 1) {
base_addr = fdt32_to_cpu(p[i]);
} else if (addr_cells == 2) {
base_addr = fdt64_to_cpu(((uint64_t) p[i + 1]) << 32 | p[i]);
}
if (size_cells == 1) {
base_size = fdt32_to_cpu(p[i + addr_cells]);
} else if (size_cells == 2) {
base_size = fdt64_to_cpu(((uint64_t) p[i + addr_cells + 1]) << 32
| p[i + addr_cells]);
}
printf("Adding memory range %16" PRIX64 " len %16" PRIX64 "\n",
base_addr, base_size);
gd_memory_map_entry ent = { 0 };
ent.type = gd_conventional_memory;
ent.attributes = 0;
ent.virtual_start = ent.physical_start = base_addr;
ent.size = base_size;
mmap_add_entry(ent);
}
dt_root = add_device_fdt(NULL, fdt, root, 0);
}
//__attribute__((no_sanitize("all")))
void kernel_start(uintptr_t magic, uintptr_t addrin)
{
kprintf("Magic %zx addrin %zx\n",magic,addrin);
__init_sanity_checks();
cpu_print_current_el();
//its a "RAM address 0"
const struct fdt_property *prop;
int addr_cells = 0, size_cells = 0;
int proplen;
//TODO find this somewhere ?.. although it is at memory 0x00
uint64_t fdt_addr=0x40000000;
char *fdt=(char*)fdt_addr;
//OK so these get overidden in the for loop which should return a map of memory and not just a single one
uint64_t addr = 0;
uint64_t size = 0;
//checks both magic and version
if ( fdt_check_header(fdt) != 0 )
{
kprint("FDT Header check failed\r\n");
return;
}
size_cells = fdt_size_cells(fdt,0);
print_le_named32("size_cells :",(char *)&size_cells);
addr_cells = fdt_address_cells(fdt, 0);//fdt32_ld((const fdt32_t *)prop->data);
print_le_named32("addr_cells :",(char *)&addr_cells);
const int mem_offset = fdt_path_offset(fdt, "/memory");
if (mem_offset < 0)
return;
print_le_named32("mem_offset :",(char *)&mem_offset);
prop = fdt_get_property(fdt, mem_offset, "reg", &proplen);
int cellslen = (int)sizeof(uint32_t) * (addr_cells + size_cells);
for (int i = 0; i < proplen / cellslen; ++i) {
for (int j = 0; j < addr_cells; ++j) {
int offset = (cellslen * i) + (sizeof(uint32_t) * j);
addr |= (uint64_t)fdt32_ld((const fdt32_t *)((char *)prop->data + offset)) <<
((addr_cells - j - 1) * 32);
}
for (int j = 0; j < size_cells; ++j) {
int offset = (cellslen * i) +
(sizeof(uint32_t) * (j + addr_cells));
size |= (uint64_t)fdt32_ld((const fdt32_t *)((char *)prop->data + offset)) <<
((size_cells - j - 1) * 32);
}
}
print_le_named64("RAM BASE :",(char *)&addr);
print_le_named64("RAM SIZE :",(char *)&size);
uint64_t mem_end=addr+size;
extern char _end;
uintptr_t free_mem_begin = reinterpret_cast<uintptr_t>(&_end);
//ok now its sane
free_mem_begin += _move_symbols(free_mem_begin);
// Initialize .bss
_init_bss();
// Instantiate machine
size_t memsize = mem_end - free_mem_begin;
__machine = os::Machine::create((void*)free_mem_begin, memsize);
_init_elf_parser();
// Begin portable HAL initialization
__machine->init();
// Initialize system calls
_init_syscalls();
//probably not very sane!
cpu_debug_enable();
cpu_fiq_enable();
cpu_irq_enable();
cpu_serror_enable();
aarch64::init_libc((uintptr_t)fdt_addr);
}