Print the device tree node names

source.elna

@@ -1,7 +1,263 @@
 module;
 
+const
+  PAGE_SIZE := 4096u;
+  FDT_BEGIN_NODE := 1u;
+  FDT_PROP := 3u;
+
+type
+  fdt_header = record
+    magic: Word;
+    totalsize: Word;
+    off_dt_struct: Word;
+    off_dt_strings: Word;
+    off_mem_rsvmap: Word;
+    version: Word;
+    last_comp_version: Word;
+    boot_cpuid_phys: Word;
+    size_dt_strings: Word;
+    size_dt_struct: Word
+  end;
+  fdt_property_header = record
+    len: Word;
+    nameoff: Word
+  end;
+
+var
+  next_paddr*: Pointer;
+
+proc write_i(value: Int); extern;
+proc write_c(value: Char); extern;
+proc write_s(value: String); extern;
+
 proc separator*() -> Char;
   return ','
 end;
 
+proc alloc_pages(n: Word) -> Pointer;
+var
+  paddr: Pointer;
+begin
+  next_paddr := next_paddr + n * PAGE_SIZE;
+
+  return paddr
+end;
+
+(*
+  Prints a number in the hexadecimal format writing the digits into the sink
+  procedure. If the value is shorter than the padding, pads the output with
+  zeroes.
+*)
+proc print_x(value: Word, padding: Word, sink: proc(Char)) -> Word;
+var
+  current: Word;
+  buffer: [8]Char;
+  length: Word;
+  written: Word;
+begin
+  length := 0u;
+  written := 0u;
+
+  while value <> 0u do
+    current := value & 15u;
+
+    if current < 10u then
+      buffer[length] := cast(current + cast('0': Word): Char)
+    else
+      buffer[length] := cast(current - 10u + cast('a': Word): Char)
+    end;
+    length := length + 1u;
+    value := value >> 4u
+  end;
+  while padding > length do
+    padding := padding - 1u;
+    sink('0');
+    written := written + 1u
+  end;
+  while length <> 0u do
+    length := length - 1u;
+    sink(buffer[length]);
+    written := written + 1u
+  end;
+  return written
+end;
+
+proc write_x(value: Word, padding: Word) -> Word;
+  return print_x(value, padding, write_c)
+end;
+
+(*
+  Converts 4 bytes of the input in network byte order into an unsigned integer.
+*)
+proc network_order(input: ^Char) -> Word;
+var
+  result: Word;
+  i: Word;
+begin
+  i := 0u;
+
+  while i < 4u do
+    result := result << 8u;
+    result := result + cast(input^: Word);
+    input := input + 1u;
+    i := i + 1u
+  end;
+
+  return result
+end;
+
+proc print_header(header: ^fdt_header);
+begin
+  write_s("Magic number: 0x");
+  write_x(header^.magic, 8u);
+  write_c('\n');
+
+  write_s("Total size: ");
+  write_i(cast(header^.totalsize: Int));
+  write_c('\n');
+
+  write_s("Struct offset: 0x");
+  write_x(header^.off_dt_struct, 8u);
+  write_c('\n');
+
+  write_s("Strings offset: 0x");
+  write_x(header^.off_dt_strings, 8u);
+  write_c('\n');
+
+  write_s("Memory reservation block offset: 0x");
+  write_x(header^.off_mem_rsvmap, 8u);
+  write_c('\n');
+
+  write_s("Version: ");
+  write_i(cast(header^.version: Int));
+  write_c('\n');
+
+  write_s("Last compatible version: ");
+  write_i(cast(header^.last_comp_version: Int));
+  write_c('\n');
+
+  write_s("CPU id: ");
+  write_i(cast(header^.boot_cpuid_phys: Int));
+  write_c('\n');
+
+  write_s("Strings length: ");
+  write_i(cast(header^.size_dt_strings: Int));
+  write_c('\n');
+
+  write_s("Struct length: ");
+  write_i(cast(header^.size_dt_struct: Int));
+  write_c('\n')
+end;
+
+proc skip_node(stream: ^Char) -> ^Char;
+var
+  token: Word;
+  property: fdt_property_header;
+begin
+  (* The first token should be FDT_BEGIN_NODE. *)
+  token := network_order(stream);
+  stream := stream + 4;
+
+  (* Read the token name. *)
+  while cast(stream^: Word) <> 0u do
+    write_c(stream^);
+    stream := stream + 1
+  end;
+  stream := stream + 1; (* Skip the trailing \0 encounted in the previous step. *)
+  write_c('\n');
+
+  while cast(stream: Word) % 4u <> 0u do
+    stream := stream + 1
+  end;
+
+  (* Property token. *)
+  token := network_order(stream);
+
+  while token = FDT_PROP do
+    stream := stream + 4;
+
+    property.len := network_order(stream);
+    stream := stream + 4;
+
+    property.nameoff := network_order(stream);
+    stream := stream + 4;
+
+    stream := stream + property.len;
+    while cast(stream: Word) % 4u <> 0u do
+      stream := stream + 1
+    end;
+
+    token := network_order(stream)
+  end;
+  while token = FDT_BEGIN_NODE do
+    stream := skip_node(stream);
+    token := network_order(stream)
+  end;
+  (* Skip FDT_END_NODE. *)
+  stream := stream + 4;
+
+  return stream
+end;
+
+proc find_memory(stream: ^Char, header: ^fdt_header);
+begin
+  stream := stream + header^.off_dt_struct;
+
+  skip_node(stream);
+
+  (* write_i(cast(stream^: Int));
+  write_i(cast(token: Int)); *)
+
+  write_c('\n')
+end;
+
+proc parse_header(stream: ^Char, header: ^fdt_header) -> ^Char;
+begin
+  header^.magic := network_order(stream);
+  stream := stream + 4;
+
+  header^.totalsize := network_order(stream);
+  stream := stream + 4;
+
+  header^.off_dt_struct := network_order(stream);
+  stream := stream + 4;
+
+  header^.off_dt_strings := network_order(stream);
+  stream := stream + 4;
+
+  header^.off_mem_rsvmap := network_order(stream);
+  stream := stream + 4;
+
+  header^.version := network_order(stream);
+  stream := stream + 4;
+
+  header^.last_comp_version := network_order(stream);
+  stream := stream + 4;
+
+  header^.boot_cpuid_phys := network_order(stream);
+  stream := stream + 4;
+
+  header^.size_dt_strings := network_order(stream);
+  stream := stream + 4;
+
+  header^.size_dt_struct := network_order(stream);
+  stream := stream + 4;
+
+  return stream
+end;
+
+proc device_tree*(raw: Pointer);
+var
+  raw_as_char: ^Char;
+  header: fdt_header;
+begin
+  raw_as_char := cast(raw: ^Char);
+
+  parse_header(raw_as_char, @header);
+  print_header(@header);
+
+  write_c('\n');
+  find_memory(raw_as_char, @header)
+end;
+
 end.