command: implement ELF loader command

Signed-off-by: Alice Ziuziakowska <a.ziuziakowska@lowrisc.org>

Author: ziuziakowska

app/commands.hh

@@ -12,6 +12,7 @@
 #include <iostream>
 #include <picosha2.h>
 #include <print>
+#include <elfio/elfio.hpp>
 
 namespace commands {
 
@@ -283,6 +284,179 @@ struct LoadFile : public Commands<T> {
   }
 };
 
+template <typename T>
+struct LoadFileElf : public Commands<T> {
+  std::string& filename;
+  bool quad;
+  bool bootstrap;
+  bool skip_erase;
+  LoadFileElf(flash::Generic<T> f, std::string& filename, bool bootstrap = false,
+              bool skip_erase = false, bool quad = false)
+      : Commands<T>(f),
+        filename(filename),
+        quad(quad),
+        bootstrap(bootstrap),
+        skip_erase(skip_erase) {}
+
+  int run() override {
+    ELFIO::elfio reader;
+    if (!reader.load(filename)) {
+      std::println("Invalid ELF file");
+      return 0;
+    }
+
+    std::vector<ELFIO::segment*> load_segments;
+    for (auto& segment : reader.segments) {
+      if (segment->get_type() == ELFIO::PT_LOAD) {
+        load_segments.push_back(&*segment);
+      }
+    }
+
+    bool addr4b     = false;
+    auto erase_size = 0;
+    auto load_size  = 0;
+    for (auto segment : load_segments) {
+      auto phys_start   = segment->get_physical_address();
+      auto phys_end     = phys_start + segment->get_memory_size();
+      auto sector_start = phys_start - (phys_start % flash::SectorSize);
+      auto sector_end   = phys_end + (flash::SectorSize - (phys_end % flash::SectorSize));
+      load_size += segment->get_file_size();
+      erase_size += (sector_end - sector_start);
+      if (phys_start > 0xFFFFFF || phys_end > 0xFFFFFF) {
+        addr4b = true;
+      }
+    }
+
+    if (!bootstrap) {
+      this->flash.reset();
+    }
+    if (addr4b && !this->flash.enter_4b_addr()) {
+      std::println("Enter 4-byte address mode failed");
+      return 0;
+    }
+    if (quad && !this->flash.enable_quad(true)) {
+      std::println("enable quad failed");
+      return 0;
+    }
+
+    this->flash.write_enable(true);
+
+    std::optional<bool> res;
+
+    // The length of the data to be loaded into memory (file size) may be less than
+    // the size of the segment in memory (memory size), in which case the excess represents
+    // zero-initialised data (e.g .bss section). Therefore, the memory size is used for
+    // erasing the flash, and then the file resident data is loaded into it, which may be
+    // shorter.
+
+    // Erase sectors containing segment data.
+    if (!skip_erase) {
+      auto erased         = 0;
+      auto erase_progress = ProgressBar(erase_size, 50, "Erasing").with_throughput();
+      for (auto segment : load_segments) {
+        auto phys_start = segment->get_physical_address();
+        auto phys_end   = phys_start + segment->get_memory_size();
+        // Align to sector size
+        auto sector_start = phys_start & ~(flash::SectorSize - 1);
+        auto sector_end   = (phys_end + flash::SectorSize - 1) & ~(flash::SectorSize - 1);
+
+        auto addr = sector_start;
+        while (addr < sector_end) {
+          res = addr4b ? this->flash.template erase<4, flash::Opcode::SectorErase4b>(addr)
+                       : this->flash.erase(addr);
+          if (!res) {
+            std::println("Failed to erase block {:#x}", addr);
+            return 0;
+          }
+
+          this->flash.wait_not_busy();
+
+          addr += flash::SectorSize;
+          erased += flash::SectorSize;
+          erase_progress.update(erased);
+        }
+      }
+    }
+
+    this->flash.wait_not_busy();
+
+    // Then, load the segment data from the file.
+    auto loaded        = 0;
+    auto load_progress = ProgressBar(load_size, 50, "Loading").with_throughput();
+    for (auto segment : load_segments) {
+      auto addr = segment->get_physical_address();
+      std::span<const uint8_t> data(reinterpret_cast<const uint8_t*>(segment->get_data()),
+                                    segment->get_file_size());
+
+      // Segments may start at an address that is not aligned to the flash page size.
+      // Page program commands may start within a page, but may wrap-around or be invalid
+      // if going over the page boundary, so only write until we are aligned to the page size.
+      if ((addr % flash::PageSize) != 0) {
+        auto to_next = flash::PageSize - (addr % flash::PageSize);
+        auto n       = std::min(to_next, data.size());
+        std::vector<uint8_t> first_n(data.first(n).begin(), data.first(n).end());
+
+        if (addr4b) {
+          res = this->flash.template single_page_program_non_blocking<4>(addr, first_n);
+        } else if (quad) {
+          res = this->flash.quad_page_program(addr, first_n);
+        } else {
+          res = this->flash.single_page_program_non_blocking(addr, first_n);
+        }
+        if (!res) {
+          std::println("Program page {:#x} failed.", addr);
+          return 0;
+        }
+
+        this->flash.wait_not_busy();
+
+        addr += n;
+        loaded += n;
+        data = data.subspan(n);
+        load_progress.update(loaded);
+      }
+
+      // Now we are aligned to the flash page size, write pages as normal.
+      while (data.size() > 0) {
+        auto n = std::min(data.size(), std::size_t{flash::PageSize});
+        std::array<uint8_t, flash::PageSize> page{0xff};
+        std::copy_n(data.begin(), n, page.begin());
+
+        if (addr4b) {
+          res = this->flash.template single_page_program_non_blocking<4>(addr, page);
+        } else if (quad) {
+          res = this->flash.quad_page_program(addr, page);
+        } else {
+          res = this->flash.single_page_program_non_blocking(addr, page);
+        }
+        if (!res) {
+          std::println("Program page {:#x} failed.", addr);
+          return 0;
+        }
+
+        this->flash.wait_not_busy();
+
+        addr += n;
+        loaded += n;
+        data = data.subspan(n);
+        load_progress.update(loaded);
+      }
+    }
+
+    this->flash.wait_not_busy();
+    this->flash.write_enable(false);
+
+    if (bootstrap) {
+      this->flash.reset();
+      return 1;
+    }
+
+    // TODO: Verifying loaded ELF files is not yet supported.
+    std::println("Verifying loaded ELF files is not yet supported!");
+    return 0;
+  }
+};
+
 template <typename T>
   requires embeddedpp::Gpio<T>
 struct GpioWrite {

app/main.cc

@@ -241,6 +241,30 @@ int main(int argc, char* argv[]) {
     return 0;
   };
 
+  auto bootstrap_elf_cmd = new_flash_command(
+      "bootstrap-elf", "Write the loadable contents of an ELF file and reset the target.");
+  bootstrap_elf_cmd->add_argument("filename").help("The file path.");
+  bootstrap_elf_cmd->add_argument("--quad")
+      .help("Use qSPI")
+      .default_value(false)
+      .implicit_value(true);
+  bootstrap_elf_cmd->add_argument("--skip-erase")
+      .help("Don't issue erase commands")
+      .default_value(false)
+      .implicit_value(true);
+  program.add_subparser(*bootstrap_elf_cmd);
+  commands["bootstrap-elf"] = [&]() -> int {
+    auto pid        = program.get<uint16_t>("--pid");
+    auto filename   = bootstrap_elf_cmd->get<std::string>("filename");
+    auto quad       = bootstrap_elf_cmd->get<bool>("--quad");
+    auto skip_erase = bootstrap_elf_cmd->get<bool>("--skip-erase");
+    auto spih       = handle_flash_command(bootstrap_elf_cmd, pid);
+    commands::LoadFileElf(flash::Generic(*spih), filename, true, skip_erase, quad).run();
+
+    spih->close();
+    return 0;
+  };
+
   auto gpio_write_cmd = new_command("gpio-write", "Write a value to an FTDI GPIO pin.");
   gpio_write_cmd->add_argument("pin").help("GPIO pin number (0-3)").required().scan<'d', int>();
   gpio_write_cmd->add_argument("value").help("Value to write (0 or 1)").required().scan<'d', int>();