New subject: [PATCH 2/5] uefi: Map uefi into arm64 system address space

23 Dec 2020

The fw_cfg device is a hardware interface allows the guest to retrieve
various data items (blobs) that can influence how the firmware configures
itself, or may contain tables to be installed for the guest OS.
Examples include device boot order, ACPI and SMBIOS tables,
virtual machine UUID, SMP and NUMA information, kernel/initrd images
for direct (Linux) kernel booting, etc.
StratoVirt will reuse the fw_cfg device model as firmware loader for
the our standard virtual machine.
Signed-off-by: Ying Fang fangying1@huawei.com
---
 device_model/src/legacy/fw_cfg.rs | 781 ++++++++++++++++++++++++++++++
 device_model/src/legacy/mod.rs    |  11 +
 device_model/src/micro_vm/mod.rs  |  24 +
 device_model/src/mmio/bus.rs      |   9 +
 device_model/src/mmio/mod.rs      |   1 +
 5 files changed, 826 insertions(+)
 create mode 100644 device_model/src/legacy/fw_cfg.rs

diff --git a/device_model/src/legacy/fw_cfg.rs b/device_model/src/legacy/fw_cfg.rs
new file mode 100644
index 0000000..7422d4b
--- /dev/null
+++ b/device_model/src/legacy/fw_cfg.rs
@@ -0,0 +1,781 @@
+// Copyright (c) 2020 Huawei Technologies Co.,Ltd. All rights reserved.
+//
+// StratoVirt is licensed under Mulan PSL v2.
+// You can use this software according to the terms and conditions of the Mulan
+// PSL v2.
+// You may obtain a copy of Mulan PSL v2 at:
+//         http://license.coscl.org.cn/MulanPSL2
+// THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY
+// KIND, EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO
+// NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
+// See the Mulan PSL v2 for more details.
+
+use super::super::mmio::errors::{Result};
+use super::super::mmio::{DeviceResource, DeviceType, MmioDeviceOps};
+use super::super::DeviceOps;
+use address_space::{AddressSpace, GuestAddress};
+use byteorder::{ByteOrder, BigEndian, LittleEndian};
+use serde::{Deserialize, Serialize};
+use kvm_ioctls::VmFd;
+use std::default::Default;
+use std::cmp::Ordering;
+use std::sync::{Arc};
+use util::byte_code::ByteCode;
+
+const FW_CFG_ACPI_DEVICE_ID: &str = "QEMU0002";
+/// QEMU CFG Signature
+const FW_CFG_DMA_SIGNATURE: u128 = 0x51454d5520434647;
+
+/// FW_CFG_VERSION bits
+const FW_CFG_VERSION: u32 = 0x01;
+const FW_CFG_VERSION_DMA: u32 = 0x02;
+const FW_CFG_FILE_SLOTS_DFLT: u32 = 0x20;
+
+enum FWCfgEntryType {
+    FW_CFG_SIGNATURE = 0x00,
+    FW_CFG_ID,
+    FW_CFG_UUID,
+    FW_CFG_RAM_SIZE,
+    FW_CFG_NOGRAPHIC,
+    FW_CFG_NB_CPUS,
+    FW_CFG_MACHINE_ID,
+    FW_CFG_KERNEL_ADDR,
+    FW_CFG_KERNEL_SIZE,
+    FW_CFG_KERNEL_CMDLINE,
+    FW_CFG_INITRD_ADDR,
+    FW_CFG_INITRD_SIZE,
+    FW_CFG_BOOT_DEVICE,
+    FW_CFG_NUMA,
+    FW_CFG_BOOT_MENU,
+    FW_CFG_MAX_CPUS,
+    FW_CFG_KERNEL_ENTRY,
+    FW_CFG_KERNEL_DATA,
+    FW_CFG_INITRD_DATA,
+    FW_CFG_CMDLINE_ADDR,
+    FW_CFG_CMDLINE_SIZE,
+    FW_CFG_SETUP_ADDR,
+    FW_CFG_SETUP_SIZE,
+    FW_CFG_FILE_DIR,
+}
+
+const FW_CFG_FILE_FIRST: u16 = 0x20;
+const FW_CFG_FILE_SLOTS_MIN: u16 = 0x10;
+const FW_CFG_WRITE_CHANNEL: u16 = 0x4000;
+const FW_CFG_ARCH_LOCAL: u16 = 0x8000;
+const FW_CFG_ENTRY_MASK: u16 = !(FW_CFG_WRITE_CHANNEL | FW_CFG_ARCH_LOCAL);
+
+const FW_CFG_INVALID: u16 = 0xffff;
+const FW_CFG_CTL_SIZE: u32 = 0x02;
+const FW_CFG_MAX_FILE_PATH: u32 = 56;
+const FW_CFG_SIG_SIZE: u32 = 4;
+
+type FWCfgCallbackType = Box<dyn FWCfgCallback + Send + Sync>;
+type FWCfgWriteCallbackType = Box<dyn FWCfgWriteCallback + Send + Sync>;
+
+pub struct FWCfgEntry {
+    data: Vec<u8>,
+    pub select_cb: Option<FWCfgCallbackType>,
+    write_cb: Option<FWCfgWriteCallbackType>,
+    allow_write: bool,
+}
+
+impl FWCfgEntry {
+    pub fn new(
+        data: Vec<u8>,
+        select_cb: Option<FWCfgCallbackType>,
+        write_cb: Option<FWCfgWriteCallbackType>,
+        allow_write: bool,
+    ) -> Self {
+        FWCfgEntry {
+            data,
+            select_cb,
+            write_cb,
+            allow_write,
+        }
+    }
+}
+
+pub trait FWCfgCallback {
+    fn select_callback(&mut self);
+}
+
+pub trait FWCfgWriteCallback {
+    fn write_callback(&mut self);
+}
+
+pub struct FWCfgFile {
+    size: u32,
+    select: u16,
+    reserved: u16,
+    name: String,
+}
+
+impl FWCfgFile {
+    pub fn new(size: u32, select: u16, reserved: u16, name: String) -> Self {
+        FWCfgFile {
+            size,
+            select,
+            reserved,
+            name,
+        }
+    }
+}
+
+impl PartialEq for FWCfgFile {
+    fn eq(&self, other: &Self) -> bool {
+        self.name.to_string() == other.name.to_string()
+    }
+}
+
+impl Eq for FWCfgFile {}
+
+impl Ord for FWCfgFile {
+    fn cmp(&self, other: &Self) -> Ordering {
+        self.name.cmp(&other.name)
+    }
+}
+
+impl PartialOrd for FWCfgFile {
+    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
+        Some(self.cmp(other))
+    }
+}
+
+const FW_CFG_VMCOREINFO_FILENAME: &str = "etc/vmcoreinfo";
+const FW_CFG_VMCOREINFO_FORMAT_NONE: u8 = 0x0;
+const FW_CFG_VMCOREINFO_FORMAT_ELF: u8 = 0x1;
+
+pub struct FWCfgVmcoreInfo {
+    host_format: u16,
+    guest_format: u16,
+    size: u32,
+    paddr: u64,
+}
+
+/// FW_CFG_DMA_CONTROL bits
+const FW_CFG_DMA_CTL_ERROR: u32 = 0x01;
+const FW_CFG_DMA_CTL_READ: u32 = 0x02;
+const FW_CFG_DMA_CTL_SKIP: u32 = 0x04;
+const FW_CFG_DMA_CTL_SELECT: u32 = 0x08;
+const FW_CFG_DMA_CTL_WRITE: u32 = 0x10;
+
+#[repr(C)]
+#[derive(Copy, Clone, Default)]
+pub struct FWCfgDmaAccess {
+    control: u32,
+    length: u32,
+    address: u64,
+}
+
+impl ByteCode for FWCfgDmaAccess {}
+
+impl FWCfgDmaAccess {
+    pub fn new(control: u32, length: u32, address: u64) -> Self {
+        FWCfgDmaAccess {
+            control,
+            length,
+            address,
+        }
+    }
+}
+
+pub fn dma_memory_set(
+    addr_space: &Arc<AddressSpace>,
+    addr: GuestAddress,
+    char: u8,
+    len: u64,
+) -> Result<()> {
+    const FILLBUF_SIZE: usize = 512;
+    let fillbuf: &[u8; FILLBUF_SIZE] = &[char; FILLBUF_SIZE];
+
+    addr_space.write(&mut fillbuf.as_ref(), addr, len)?;
+    Ok(())
+}
+
+pub fn dma_memory_write(
+    addr_space: &Arc<AddressSpace>,
+    addr: GuestAddress,
+    mut buf: &[u8],
+    len: u64,
+) -> Result<()> {
+    addr_space.write(&mut buf, addr, len)?;
+    Ok(())
+}
+
+pub fn dma_memory_read(
+    addr_space: &Arc<AddressSpace>,
+    addr: GuestAddress,
+    mut buf: &mut [u8],
+    len: u64,
+) -> Result<()> {
+    addr_space.read(&mut buf, addr, len)?;
+    Ok(())
+}
+
+pub fn dma_result_write(
+    addr_space: &Arc<AddressSpace>,
+    addr: GuestAddress,
+    value: u32,
+) -> Result<()> {
+    let mut dma_buf: [u8; 4] = [0; 4];
+    BigEndian::write_u32(&mut dma_buf, value);
+    dma_memory_write(
+        addr_space,
+        addr.unchecked_add(offset_of!(FWCfgDmaAccess, control) as u64),
+        &dma_buf,
+        dma_buf.len() as u64,
+    )?;
+    Ok(())
+}
+
+fn extract64(value: u64, start: i32, length: i32) -> u64 {
+    assert!(start >= 0 && length > 0 && length < 64 - start);
+
+    (value >> start as u64) & (!(0 as u64) >> (64 - length) as u64)
+}
+
+fn get_key_name(key: usize) -> &'static str {
+    static fw_cfg_keys: [&str; 25] = [
+        "signature",
+        "id",
+        "uuid",
+        "ram_size",
+        "nographic",
+        "nb_cpus",
+        "machine_id",
+        "kernel_addr",
+        "kernel_size",
+        "kernel_cmdline",
+        "initrd_size",
+        "boot_device",
+        "numa",
+        "boot_menu",
+        "max_cpus",
+        "kernel_entry",
+        "kernel_data",
+        "initrd_data",
+        "cmdline_addr",
+        "cmdline_size",
+        "cmdline_data",
+        "setup_addr",
+        "setup_size",
+        "setup_data",
+        "file_dir",
+    ];
+    return fw_cfg_keys[key];
+}
+
+pub struct FWCfgCommon {
+    file_slots: u16,
+    local_entries: Vec<FWCfgEntry>,
+    entries: Vec<FWCfgEntry>,
+    files: Vec<FWCfgFile>,
+    cur_entry: u16,
+    cur_offset: u32,
+
+    dma_enabled: bool,
+    dma_addr: GuestAddress,
+
+    /// System address space.
+    mem_space: Arc<AddressSpace>,
+}
+
+impl FWCfgCommon {
+    pub fn new(sys_mem: Arc<AddressSpace>) -> Self {
+        FWCfgCommon {
+            file_slots: FW_CFG_FILE_SLOTS_DFLT as u16,
+            local_entries: Vec::new(),
+            entries: Vec::new(),
+            files: Vec::new(),
+            cur_entry: 0,
+            cur_offset: 0,
+            dma_enabled: true,
+            dma_addr: GuestAddress(0),
+            mem_space: sys_mem,
+        }
+    }
+
+    fn fw_cfg_max_entry(&self) -> u16 {
+        FW_CFG_FILE_FIRST + self.file_slots
+    }
+
+    ///  QEMU v2.4+ does not support write anymore
+    fn fw_cfg_write(&self, _value: u8) {}
+
+    pub fn fw_cfg_get_entry(&mut self) -> Option<&mut FWCfgEntry> {
+        let is_local = (self.cur_entry & FW_CFG_ARCH_LOCAL as u16) > 0;
+
+        let mut entry = None;
+        if self.cur_entry != FW_CFG_INVALID {
+            if is_local {
+                entry = self
+                    .local_entries
+                    .get_mut((self.cur_entry & FW_CFG_ENTRY_MASK) as usize)
+            } else {
+                entry = self
+                    .entries
+                    .get_mut((self.cur_entry & FW_CFG_ENTRY_MASK) as usize)
+            }
+        }
+        entry
+    }
+
+    pub fn fw_cfg_select(&mut self, key: u16) -> i32 {
+        let mut ret = 0;
+
+        self.cur_offset = 0;
+        if (key & FW_CFG_ENTRY_MASK) >= self.fw_cfg_max_entry() {
+            self.cur_entry = FW_CFG_INVALID;
+            ret = 0;
+        } else {
+            self.cur_entry = key;
+            ret = 1;
+
+            // TODO Invoke callback here
+            let entry = self.fw_cfg_get_entry().unwrap();
+            entry.select_cb.as_mut().unwrap().select_callback();
+        }
+
+        debug!("fw_cfg_select key {} ret {}", key, ret);
+        ret
+    }
+
+    fn fw_cfg_dma_transfer(&mut self) {
+        let dma_addr = self.dma_addr;
+        let mem_space = self.mem_space.clone();
+        let mut cur_offset = self.cur_offset;
+        let cur_entry = self.cur_entry;
+
+        // Reset dma_addr address before next dma access
+        self.dma_addr = GuestAddress(0);
+
+        let mut default = FWCfgDmaAccess::default();
+        let mut dma_buf = default.as_mut_bytes();
+
+        // Read DMA request from guest
+        let size = std::mem::size_of::<FWCfgDmaAccess>() as u64;
+        if let Err(_) = dma_memory_read(&self.mem_space, dma_addr, dma_buf, size) {
+            dma_result_write(&self.mem_space, dma_addr, FW_CFG_DMA_CTL_ERROR);
+        }
+
+        // Build FWCfgDmaAccess object here
+        let dma = FWCfgDmaAccess::from_mut_bytes(&mut dma_buf).unwrap();
+
+        if dma.control & FW_CFG_DMA_CTL_SELECT > 0 {
+            self.fw_cfg_select((dma.control >> 16) as u16);
+        }
+
+        // Note a mutable borrow here
+        let entry = self.fw_cfg_get_entry().unwrap();
+        let mut read = false;
+        let mut write = false;
+
+        if dma.control & FW_CFG_DMA_CTL_READ != 0 {
+            read = true;
+            write = false;
+        } else if dma.control & FW_CFG_DMA_CTL_WRITE != 0 {
+            read = false;
+            write = true;
+        } else if dma.control & FW_CFG_DMA_CTL_SKIP != 0 {
+            dma.length = 0;
+        }
+
+        dma.control = 0;
+        let mut len: u32 = 0;
+
+        while dma.length > 0 && (dma.control & FW_CFG_DMA_CTL_ERROR) != 0 {
+            if cur_entry == FW_CFG_INVALID
+                || !entry.data.is_empty()
+                || cur_offset >= entry.data.len() as u32
+            {
+                len = dma.length;
+            }
+
+            if read {
+                if let Err(_) = dma_memory_set(&mem_space, GuestAddress(dma.address), 0, len as u64)
+                {
+                    dma.control |= FW_CFG_DMA_CTL_ERROR;
+                }
+
+                if write {
+                    dma.control |= FW_CFG_DMA_CTL_ERROR;
+                }
+            } else {
+                if dma.length <= entry.data.len() as u32 - cur_offset {
+                    len = dma.length;
+                } else {
+                    len = entry.data.len() as u32 - cur_offset;
+                }
+
+                if read {
+                    if let Err(_) = dma_memory_write(
+                        &mem_space,
+                        GuestAddress(dma.address),
+                        &entry.data[cur_offset as usize..],
+                        len as u64,
+                    ) {
+                        dma.control |= FW_CFG_DMA_CTL_ERROR;
+                    }
+                }
+
+                if write {
+                    let mut dma_read_error = false;
+                    let data = &mut entry.data[cur_offset as usize..];
+                    if let Err(_) =
+                    dma_memory_read(&mem_space, GuestAddress(dma.address), data, len as u64)
+                    {
+                        dma_read_error = true;
+                    }
+
+                    if dma_read_error || !entry.allow_write || len != dma.length {
+                        dma.control |= FW_CFG_DMA_CTL_ERROR;
+                    } else if true {
+                        // TODO invoke write callback
+                    }
+                }
+                cur_offset += len;
+            }
+
+            dma.length += len;
+            dma.address += len as u64
+        }
+        dma_result_write(&self.mem_space, dma_addr, dma.control);
+        debug!("fw_cfg dma tranfer");
+    }
+
+    pub fn fw_cfg_add_entry(
+        &mut self,
+        key: u16,
+        select_cb: Option<FWCfgCallbackType>,
+        write_cb: Option<FWCfgWriteCallbackType>,
+        data: Vec<u8>,
+        allow_write: bool,
+    ) {
+        let is_local = key & FW_CFG_ARCH_LOCAL > 0;
+
+        assert!(key < self.fw_cfg_max_entry() && data.len() < std::u32::MAX as usize);
+
+        let mut entry;
+        if is_local {
+            entry = &mut self.local_entries[key as usize];
+        } else {
+            entry = &mut self.entries[key as usize];
+        }
+
+        entry.data = data;
+        entry.select_cb = select_cb;
+        entry.write_cb = write_cb;
+        entry.allow_write = allow_write;
+    }
+
+    pub fn fw_cfg_update_entry_data(&mut self, key: u16, data: Vec<u8>) {
+        let is_local = key & FW_CFG_ARCH_LOCAL > 0;
+
+        assert!(key < self.fw_cfg_max_entry() && data.len() < std::u32::MAX as usize);
+
+        let mut entry;
+        if is_local {
+            entry = &mut self.local_entries[key as usize];
+        } else {
+            entry = &mut self.entries[key as usize];
+        }
+
+        entry.data = data;
+        entry.allow_write = false;
+    }
+
+    pub fn fw_cfg_add_entry_data(&mut self, key: u16, data: Vec<u8>) {
+        debug!("fw_cfg_add_data key {} ", get_key_name(key as usize));
+
+        self.fw_cfg_add_entry(key, None, None, data, true);
+    }
+
+    pub fn fw_cfg_entry_add_string(&mut self, key: u16, value: &str) {
+        self.fw_cfg_update_entry_data(key, value.as_bytes().to_vec());
+    }
+
+    pub fn fw_cfg_add_file_callback(&mut self,
+                                    filename: &str,
+                                    data: Vec<u8>,
+                                    select_cb: Option<FWCfgCallbackType>,
+                                    write_cb: Option<FWCfgWriteCallbackType>,
+                                    allow_write: bool) -> Result<()> {
+        let len = self.files.len();
+        assert!(len < self.file_slots as usize);
+        let mut index = len;
+        loop {
+            if filename.to_string() < self.files[index - 1].name.to_string() {
+                index -= 1;
+            } else {
+                break;
+            }
+        }
+
+        let mut i = 0;
+        // check duplicate file
+        for f in self.files.iter() {
+            if f.name == filename && i != index {
+                bail!("duplicate fw_cfg file name {}", filename);
+            }
+            i += 1;
+        }
+
+        let entry = FWCfgEntry::new(data, select_cb, write_cb, allow_write);
+        self.local_entries.insert(index + FW_CFG_FILE_FIRST as usize, entry);
+        let file = FWCfgFile::new(BigEndian::read_u32((len as u32).as_bytes()),
+                                  BigEndian::read_u16(((index + FW_CFG_FILE_FIRST as usize) as u16).as_bytes()),
+                                  0, filename.to_string());
+        self.files.push(file);
+        self.files.sort();
+        Ok(())
+    }
+
+    pub fn fw_cfg_add_file(&mut self, filename: &str, data: Vec<u8>) {
+        self.fw_cfg_add_file_callback(filename, data, None, None, false);
+    }
+
+    pub fn fw_cfg_dma_mem_write(&mut self, addr: GuestAddress, value: u64, size: u32) {
+        if size == 4 {
+            if addr == GuestAddress(0) {
+                self.dma_addr = GuestAddress(value << 32);
+            } else if addr == GuestAddress(4) {
+                self.dma_addr = GuestAddress(self.dma_addr.raw_value() | size as u64);
+                self.fw_cfg_dma_transfer();
+            }
+        } else if size == 8 && addr == GuestAddress(0) {
+            self.dma_addr = GuestAddress(value);
+            self.fw_cfg_dma_transfer();
+        }
+    }
+
+    pub fn fw_cfg_dma_mem_read(&self, addr: GuestAddress, size: u32) -> u64 {
+        extract64(
+            FW_CFG_DMA_SIGNATURE as u64,
+            ((8 - addr.raw_value() - size as u64) * 8) as i32,
+            (size * 8) as i32,
+        )
+    }
+
+    pub fn fw_cfg_data_read(&mut self, addr: GuestAddress, mut size: u32) -> u64 {
+        let mut value: u64 = 0;
+        let cur_entry = self.cur_entry;
+        let cur_offset = self.cur_offset;
+
+        let entry = self.fw_cfg_get_entry().unwrap();
+        let SIZE_U64 = std::mem::size_of::<u64>() as u32;
+        assert!(size > 0 && size < SIZE_U64);
+        if cur_entry != FW_CFG_INVALID
+            && !entry.data.is_empty()
+            && cur_offset < entry.data.len() as u32
+        {
+            let mut offset = cur_offset;
+            loop {
+                value = (value << 8) | entry.data[offset as usize] as u64;
+                offset += 1;
+                size -= 1;
+
+                if size < 0 || offset >= entry.data.len() as u32 {
+                    break;
+                }
+            }
+            value = 8 * size as u64;
+        }
+
+        debug!(
+            "fw_cfg_data_read 0x{:x} size {} value 0x{:x}",
+            addr.raw_value(),
+            size,
+            value
+        );
+        value
+    }
+
+    pub fn fw_cfg_common_realize(&mut self) {
+        self.local_entries = Vec::with_capacity(self.fw_cfg_max_entry() as usize);
+        self.entries = Vec::with_capacity(self.fw_cfg_max_entry() as usize);
+
+        self.fw_cfg_add_entry_data(
+            FWCfgEntryType::FW_CFG_SIGNATURE as u16,
+            "QEMU".as_bytes().to_vec(),
+        );
+
+        // TODO add real uuid here
+        self.fw_cfg_add_entry_data(
+            FWCfgEntryType::FW_CFG_UUID as u16,
+            "49c9bedd6459b52a".as_bytes().to_vec(),
+        );
+
+        self.fw_cfg_add_entry_data(
+            FWCfgEntryType::FW_CFG_NOGRAPHIC as u16,
+            (0 as u16).as_bytes().to_vec(),
+        );
+
+        self.fw_cfg_add_entry_data(
+            FWCfgEntryType::FW_CFG_BOOT_MENU as u16,
+            (0 as u16).as_bytes().to_vec(),
+        );
+
+        let version = FW_CFG_VERSION & FW_CFG_VERSION_DMA;
+        self.fw_cfg_add_entry_data(
+            FWCfgEntryType::FW_CFG_ID as u16,
+            version.as_bytes().to_vec(),
+        );
+    }
+}
+
+#[derive(Debug, Clone, Default, Serialize, Deserialize)]
+pub struct FWCfgConfig {}
+
+/// Emulate FWCfg MMIO Device
+pub struct FWCfgMem {
+    fwcfg: FWCfgCommon,
+    data_width: u32,
+}
+
+
+impl FWCfgMem {
+    pub fn new(sys_mem: Arc<AddressSpace>) -> Self {
+        FWCfgMem {
+            fwcfg: FWCfgCommon::new(sys_mem),
+            data_width: 8,
+        }
+    }
+
+    pub fn fw_cfg_data_mem_write(&self, _addr: GuestAddress, value: u64, size: u32) {
+        let mut i = size;
+        loop {
+            i -= 1;
+            self.fwcfg.fw_cfg_write((value >> (8 * i) as u64) as u8);
+            if i == 0 {
+                break;
+            }
+        }
+    }
+
+    pub fn fw_cfg_ctl_mem_read(&self, _addr: GuestAddress, _size: u32) -> u64 {
+        0
+    }
+
+    pub fn fw_cfg_ctl_mem_write(&mut self, _addr: GuestAddress, value: u64, _size: u32) {
+        self.fwcfg.fw_cfg_select(value as u16);
+    }
+}
+
+impl DeviceOps for FWCfgMem {
+    fn read(&mut self, data: &mut [u8], base: GuestAddress, offset: u64) -> bool {
+        let mut value = 0;
+        match offset {
+            0..=7 => {
+                value = self.fwcfg.fw_cfg_data_read(base, data.len() as u32);
+            }
+            8..=15 => {
+                value = self.fw_cfg_ctl_mem_read(base, data.len() as u32);
+            }
+            _ => {
+                self.fwcfg.fw_cfg_dma_mem_read(base, data.len() as u32);
+            }
+        }
+
+        BigEndian::write_u64(data, value);
+        true
+    }
+
+    fn write(&mut self, data: &[u8], base: GuestAddress, offset: u64) -> bool {
+        let mut value = 0;
+        let size = data.len() as u32;
+        value = BigEndian::read_u64(data);
+        match offset {
+            0..=7 => {
+                self.fw_cfg_data_mem_write(base, value, size);
+            }
+            8..=15 => {
+                self.fw_cfg_ctl_mem_write(base, value, size);
+            }
+            _ => {
+                self.fwcfg.fw_cfg_dma_mem_write(base, value, size);
+            }
+        }
+        true
+    }
+}
+
+impl MmioDeviceOps for FWCfgMem {
+    /// Realize FWCfgMem device when VM started.
+    fn realize(&mut self, _vm_fd: &VmFd, _resource: DeviceResource) -> Result<()> {
+        self.fwcfg.fw_cfg_common_realize();
+        Ok(())
+    }
+
+    /// Get device type.
+    fn get_type(&self) -> DeviceType {
+        DeviceType::FWCFG
+    }
+}
+
+/// Emulate FWCfg IO device
+pub struct FWCfgIO {
+    fwcfg: FWCfgCommon,
+}
+
+impl FWCfgIO {
+    pub fn new(sys_mem: Arc<AddressSpace>) -> Self {
+        FWCfgIO {
+            fwcfg: FWCfgCommon {
+                file_slots: FW_CFG_FILE_SLOTS_DFLT as u16,
+                local_entries: Vec::new(),
+                entries: Vec::new(),
+                files: Vec::new(),
+                cur_entry: 0,
+                cur_offset: 0,
+                dma_enabled: true,
+                dma_addr: GuestAddress(0),
+                mem_space: sys_mem,
+            },
+        }
+    }
+
+    pub fn fw_cfg_comb_write(&mut self, _addr: GuestAddress, value: u64, size: u32) {
+        match size {
+            1 => {
+                self.fwcfg.fw_cfg_write(value as u8);
+            }
+            2 => {
+                self.fwcfg.fw_cfg_select(value as u16);
+            }
+            _ => return,
+        }
+    }
+}
+
+impl DeviceOps for FWCfgIO {
+    fn read(&mut self, data: &mut [u8], base: GuestAddress, offset: u64) -> bool {
+        let mut value = 0;
+        match offset {
+            1..=2 => {
+                value = self.fwcfg.fw_cfg_data_read(base, data.len() as u32);
+            }
+            15..=23 => {
+                value = self.fwcfg.fw_cfg_dma_mem_read(base, data.len() as u32);
+            }
+            _ => error!("invalid read of set in FWCfgIO"),
+        }
+        LittleEndian::write_u64(data, value);
+        true
+    }
+
+    fn write(&mut self, data: &[u8], base: GuestAddress, offset: u64) -> bool {
+        let mut value = 0;
+        let size = data.len() as u32;
+        value = LittleEndian::read_u64(data);
+        match offset {
+            1..=2 => {
+                self.fw_cfg_comb_write(base, value, size);
+            }
+            15..=23 => {
+                self.fwcfg.fw_cfg_dma_mem_write(base, value, size);
+            }
+            _ => {}
+        }
+        true
+    }
+}
diff --git a/device_model/src/legacy/mod.rs b/device_model/src/legacy/mod.rs
index f840656..bd8970e 100644
--- a/device_model/src/legacy/mod.rs
+++ b/device_model/src/legacy/mod.rs
@@ -25,9 +25,20 @@
 //! - `x86_64`
 //! - `aarch64`
 mod serial;
+
 pub use self::serial::Serial;
#[cfg(target_arch = "aarch64")]
 mod pl031;
+
 #[cfg(target_arch = "aarch64")]
 pub use self::pl031::PL031;
+
+#[cfg(target_arch = "aarch64")]
+mod fw_cfg;
+
+#[cfg(target_arch = "aarch64")]
+pub use self::fw_cfg::FWCfgMem;
+#[cfg(target_arch = "aarch64")]
+pub use self::fw_cfg::FWCfgConfig;
+
diff --git a/device_model/src/micro_vm/mod.rs b/device_model/src/micro_vm/mod.rs
index 94a3e3f..6751f8d 100644
--- a/device_model/src/micro_vm/mod.rs
+++ b/device_model/src/micro_vm/mod.rs
@@ -983,6 +983,26 @@ impl MainLoopManager for LightMachine {
     }
 }
+/// Function that helps to generate fw-cfg node in device-tree.
+///
+/// # Arguments
+///
+/// * `dev_info` - Device resource info of fw-cfg device.
+/// * `fdt` - Flatted device-tree blob where fw-cfg node will be filled into.
+#[cfg(target_arch = "aarch64")]
+fn generate_fwcfg_device_node(
+    dev_info: &DeviceResource,
+    fdt: &mut Vec<u8>,
+) -> util::errors::Result<()> {
+    let node = format!("/fw-cfg@{:x}", dev_info.addr);
+    device_tree::add_sub_node(fdt, &node)?;
+    device_tree::set_property_string(fdt, &node, "compatible", "qemu,fw-cfg-mmio")?;
+    device_tree::set_property_array_u64(fdt, &node, "reg", &[dev_info.addr, dev_info.size])?;
+
+    Ok(())
+}
+
+
 /// Function that helps to generate serial node in device-tree.
 ///
 /// # Arguments
@@ -1216,6 +1236,7 @@ impl CompileFDTHelper for LightMachine {
         device_tree::set_property_string(fdt, node, "compatible", "arm,psci-0.2")?;
         device_tree::set_property_string(fdt, node, "method", "hvc")?;
+
         for dev_info in self.bus.get_devices_info().iter().rev() {
             match dev_info.dev_type {
                 DeviceType::SERIAL => {
@@ -1224,6 +1245,9 @@ impl CompileFDTHelper for LightMachine {
                 DeviceType::RTC => {
                     generate_rtc_device_node(dev_info, fdt)?;
                 }
+                DeviceType::FWCFG => {
+                    generate_fwcfg_device_node(dev_info, fdt)?;
+                }
                 _ => {
                     generate_virtio_devices_node(dev_info, fdt)?;
                 }
diff --git a/device_model/src/mmio/bus.rs b/device_model/src/mmio/bus.rs
index 79132b9..3fe8c7c 100644
--- a/device_model/src/mmio/bus.rs
+++ b/device_model/src/mmio/bus.rs
@@ -184,6 +184,15 @@ impl Bus {
                     }
                 }
             }
+            DeviceType::FWCFG => {
+                #[cfg(target_arch = "aarch64")]
+                DeviceResource {
+                    addr: MEM_LAYOUT[LayoutEntryType::FwCfg as usize].0,
+                    size: MEM_LAYOUT[LayoutEntryType::FwCfg as usize].1,
+                    irq: 0,
+                    dev_type: device_type,
+                }
+            }
             _ => DeviceResource {
                 addr: MMIO_BASE + index as u64 * MMIO_LEN,
                 size: MMIO_LEN,
diff --git a/device_model/src/mmio/mod.rs b/device_model/src/mmio/mod.rs
index d2f6295..4ac532f 100644
--- a/device_model/src/mmio/mod.rs
+++ b/device_model/src/mmio/mod.rs
@@ -65,6 +65,7 @@ pub enum DeviceType {
     SERIAL,
     #[cfg(target_arch = "aarch64")]
     RTC,
+    FWCFG,
     OTHER,
 }
-- 
2.23.0

    

[PATCH 1/5] fw_cfg: Add QEMU Firmware Configuration Device