[RFC,v0,4/6] Discover QEMU fwcfg device and use it to load the kernel

diff --git a/src/fwcfg.rs b/src/fwcfg.rs
new file mode 100644
index 000000000000..57f405df174b
--- /dev/null
+++ b/src/fwcfg.rs
@@ -0,0 +1,85 @@ 
+/* SPDX-License-Identifier: GPL-2.0 */
+
+use mmio::{Allow, Deny, VolBox};
+
+pub struct FwCfg {
+    // read-only data register
+    data: VolBox<u64, Allow, Deny>,
+
+    // write-only selector register
+    selector: VolBox<u16, Deny, Allow>,
+
+    // write-only DMA register
+    dmacontrol: VolBox<u64, Deny, Allow>,
+}
+
+const CFG_KERNEL_SIZE: u16 = 0x08;
+const CFG_KERNEL_DATA: u16 = 0x11;
+
+const CFG_DMACTL_DONE: u32 = 0;
+const CFG_DMACTL_ERROR: u32 = 1;
+const CFG_DMACTL_READ: u32 = 2;
+
+#[repr(C)]
+struct DmaTransfer {
+    control: u32,
+    length: u32,
+    address: u64,
+}
+
+impl FwCfg {
+    pub fn from_fdt_node(node: fdt::node::FdtNode) -> Option<FwCfg> {
+        if let Some(mut iter) = node.reg() {
+            iter.next().map(|reg| {
+                let addr = reg.starting_address;
+                unsafe {
+                    FwCfg {
+                        data: VolBox::<u64, Allow, Deny>::new(addr as *mut u64),
+                        selector: VolBox::<u16, Deny, Allow>::new(addr.offset(8) as *mut u16),
+                        dmacontrol: VolBox::<u64, Deny, Allow>::new(addr.offset(16) as *mut u64),
+                    }
+                }
+            })
+        } else {
+            None
+        }
+    }
+
+    unsafe fn dma_transfer(
+        &mut self,
+        load_address: *mut u8,
+        size: usize,
+        config_item: u16,
+    ) -> Result<(), &str> {
+        let xfer = DmaTransfer {
+            control: u32::to_be(CFG_DMACTL_READ),
+            length: u32::to_be(size as u32),
+            address: u64::to_be(load_address as u64),
+        };
+        self.selector.write(u16::to_be(config_item));
+        self.dmacontrol.write(u64::to_be(&xfer as *const _ as u64));
+
+        let control = VolBox::<u32, Allow, Deny>::new(&xfer.control as *const _ as *mut u32);
+        loop {
+            match control.read() {
+                CFG_DMACTL_DONE => return Ok(()),
+                CFG_DMACTL_ERROR => return Err("fwcfg DMA error"),
+                _ => (), // keep polling
+            }
+        }
+    }
+
+    pub fn get_kernel_size(&mut self) -> usize {
+        self.selector.write(u16::to_be(CFG_KERNEL_SIZE));
+        self.data.read() as usize
+    }
+
+    pub fn load_kernel_image(&mut self, load_address: *mut u8) -> Result<(), &str> {
+        let size = self.get_kernel_size();
+        if size > 0 {
+            unsafe { self.dma_transfer(load_address, size, CFG_KERNEL_DATA) }
+        } else {
+            Err("No kernel image provided by fwcfg")
+        }
+    }
+}
diff --git a/src/main.rs b/src/main.rs
index af58ccc0318d..048d1b4842cb 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -8,6 +8,7 @@ 
 
 mod console;
 mod cstring;
+mod fwcfg;
 mod pagealloc;
 mod paging;
 
@@ -28,6 +29,8 @@  extern "C" {
     static _dtb_end: u8;
 }
 
+const LOAD_ADDRESS: *mut u8 = 0x43210000 as _;
+
 #[no_mangle]
 extern "C" fn efilite_main(base: usize, mapped: usize, used: usize) {
     #[cfg(debug_assertions)]
@@ -79,6 +82,21 @@  extern "C" fn efilite_main(base: usize, mapped: usize, used: usize) {
     // Switch to the new ID map so we can use all of DRAM
     paging::activate();
 
+    let compat = ["qemu,fw-cfg-mmio"];
+    let fwcfg_node = fdt
+        .find_compatible(&compat)
+        .expect("QEMU fwcfg node not found");
+
+    info!("QEMU fwcfg node found: {}\n", fwcfg_node.name);
+
+    let mut fwcfg = fwcfg::FwCfg::from_fdt_node(fwcfg_node).expect("Failed to open fwcfg device");
+
+    // TODO allocate fwcfg.get_kernel_size() bytes here instead of using a fixed address
+
+    fwcfg
+        .load_kernel_image(LOAD_ADDRESS)
+        .expect("Failed to load kernel image");
+
     // Switch back to the initial ID map so we can remap
     // the loaded kernel image with different permissions
     paging::deactivate();