Signed-off-by: Ying Fang fangying1@huawei.com --- device_model/src/legacy/fw_cfg.rs | 160 ++++++++++++++++++------------ device_model/src/legacy/pl011.rs | 8 +- 2 files changed, 99 insertions(+), 69 deletions(-)
diff --git a/device_model/src/legacy/fw_cfg.rs b/device_model/src/legacy/fw_cfg.rs index 09396cc..04e4862 100644 --- a/device_model/src/legacy/fw_cfg.rs +++ b/device_model/src/legacy/fw_cfg.rs @@ -14,9 +14,11 @@ use super::super::mmio::errors::Result; use super::super::mmio::{DeviceResource, DeviceType, MmioDeviceOps}; use super::super::DeviceOps; use address_space::{AddressSpace, GuestAddress}; -use byteorder::{BigEndian, ByteOrder, LittleEndian}; use kvm_ioctls::VmFd; + +use byteorder::{BigEndian, ByteOrder, LittleEndian}; use std::cmp::Ordering; +use std::collections::BTreeMap; use std::default::Default; use std::sync::Arc; use util::byte_code::ByteCode; @@ -268,9 +270,9 @@ fn get_key_name(key: usize) -> &'static str {
pub struct FWCfgCommon { file_slots: u16, - local_entries: Vec<FWCfgEntry>, - entries: Vec<FWCfgEntry>, - files: Vec<FWCfgFile>, + local_entries: BTreeMap<u16, FWCfgEntry>, + entries: BTreeMap<u16, FWCfgEntry>, + files: BTreeMap<String, FWCfgFile>, cur_entry: u16, cur_offset: u32,
@@ -285,9 +287,9 @@ 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(), + local_entries: BTreeMap::new(), + entries: BTreeMap::new(), + files: BTreeMap::new(), cur_entry: 0, cur_offset: 0, dma_enabled: true, @@ -304,26 +306,24 @@ impl FWCfgCommon { fn fw_cfg_write(&self, _value: u8) {}
pub fn fw_cfg_get_entry(&mut self) -> Option<&mut FWCfgEntry> { - info!("fw_cfg_get_entry"); - let is_local = (self.cur_entry & FW_CFG_ARCH_LOCAL as u16) > 0; + let key = self.cur_entry & FW_CFG_ENTRY_MASK; + info!("fw_cfg_get_entry 0x{:x}", key); + let is_local = (self.cur_entry & FW_CFG_ARCH_LOCAL) == 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) + entry = self.local_entries.get_mut(&key) } else { - entry = self - .entries - .get_mut((self.cur_entry & FW_CFG_ENTRY_MASK) as usize) + entry = self.entries.get_mut(&key); } } entry }
pub fn fw_cfg_select(&mut self, key: u16) -> i32 { - info!("fw_cfg_select"); + info!("fw_cfg_select {}", get_key_name(key as usize)); let mut ret = 0;
self.cur_offset = 0; @@ -336,7 +336,11 @@ impl FWCfgCommon {
// TODO Invoke callback here let entry = self.fw_cfg_get_entry().unwrap(); - entry.select_cb.as_mut().unwrap().select_callback(); + if let Some(ref mut cb) = &mut entry.select_cb { + cb.select_callback(); + } else { + info!("no callback func found!"); + } }
info!("fw_cfg_select key {} ret {}", key, ret); @@ -456,28 +460,29 @@ impl FWCfgCommon { data: Vec<u8>, allow_write: bool, ) { - info!("fw_cfg_add_entry {}", get_key_name(key as usize)); - let is_local = key & FW_CFG_ARCH_LOCAL > 0; - - assert!(key < self.fw_cfg_max_entry() && data.len() < std::u32::MAX as usize); + let is_local = key & FW_CFG_ARCH_LOCAL == 0;
key &= FW_CFG_ENTRY_MASK; + assert!(key < self.fw_cfg_max_entry() && data.len() < std::u32::MAX as usize);
let mut entry; if is_local { - entry = self.local_entries.get_mut(key as usize); + entry = self.local_entries.get_mut(&key); } else { - entry = self.entries.get_mut(key as usize); + entry = self.entries.get_mut(&key); }
if let Some(ref mut e) = entry { - e.data = data; - e.select_cb = select_cb; - e.write_cb = write_cb; - e.allow_write = allow_write; + error!("entry {} is not empty!", get_key_name(key as usize)); + } + + let entry = FWCfgEntry::new(data, select_cb, write_cb, allow_write); + if is_local { + self.local_entries.insert(key, entry); } else { - // *e = FWCfgEntry::new(data, select_cb, write_cb, allow_write); + self.entries.insert(key, entry); } + info!("fw_cfg_add_entry index 0x{:x} done!", key); }
pub fn fw_cfg_update_entry_data(&mut self, key: u16, data: Vec<u8>) { @@ -491,18 +496,17 @@ impl FWCfgCommon {
let mut entry; if is_local { - entry = &mut self.local_entries[key as usize]; + entry = &mut self.local_entries.get_mut(&key); } else { - entry = &mut self.entries[key as usize]; + entry = &mut self.entries.get_mut(&key); }
- entry.data = data; - entry.allow_write = false; + // entry.data = data; + // entry.allow_write = false; }
pub fn fw_cfg_add_entry_data(&mut self, key: u16, data: Vec<u8>) { - info!("fw_cfg_add_data key {} ", get_key_name(key as usize)); - + info!("fw_cfg_add_entry_data key {}", get_key_name(key as usize)); self.fw_cfg_add_entry(key, None, None, data, false); }
@@ -520,42 +524,50 @@ impl FWCfgCommon { allow_write: bool, ) -> Result<()> { info!("fw_cfg_add_file_callback filename {}", filename); - 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; - } + + assert!(self.files.len() < self.file_slots as usize); + + // check against duplicate file + if self.files.contains_key(&filename.to_string()) { + bail!("duplicate fw_cfg file name {}", filename); }
- 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); + let tmp = FWCfgFile::new( + BigEndian::read_u32((data.len() as u32).as_bytes()), + BigEndian::read_u16(FW_CFG_FILE_FIRST.as_bytes()), + 0, + filename.to_string(), + ); + self.files.insert(filename.to_string(), tmp); + + let mut index = 0; + for (key, value) in self.files.iter() { + if *key == filename.to_string() { + break; } - i += 1; + index += 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()), + BigEndian::read_u32((data.len() as u32).as_bytes()), + BigEndian::read_u16((FW_CFG_FILE_FIRST + index).as_bytes()), 0, filename.to_string(), ); - self.files.push(file); - self.files.sort(); + self.files.insert(filename.to_string(), file); + + self.fw_cfg_add_entry( + FW_CFG_FILE_FIRST + index, + select_cb, + write_cb, + data, + allow_write, + ); 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); + self.fw_cfg_add_file_callback(filename, data, None, None, true); }
pub fn fw_cfg_dma_mem_write(&mut self, addr: u64, value: u64, size: u32) { @@ -593,6 +605,10 @@ impl FWCfgCommon { let entry = self.fw_cfg_get_entry().unwrap(); let size_u64 = std::mem::size_of::<u64>() as u32; assert!(size > 0 && size < size_u64); + + info!("entry data len is {} cur_offset {}", entry.data.len(), cur_offset); + info!("entry data is {:?}", entry.data); + if cur_entry != FW_CFG_INVALID && !entry.data.is_empty() && cur_offset < entry.data.len() as u32 @@ -607,7 +623,7 @@ impl FWCfgCommon { break; } } - value = 8 * size as u64; + value <<= 8 * size as u64; }
info!( @@ -618,7 +634,6 @@ impl FWCfgCommon { }
pub fn fw_cfg_common_realize(&mut self) { - self.fw_cfg_add_entry_data( FWCfgEntryType::FWCfgSignature as u16, "QEMU".as_bytes().to_vec(), @@ -640,7 +655,13 @@ impl FWCfgCommon { (0 as u16).as_bytes().to_vec(), );
- let version = FW_CFG_VERSION & FW_CFG_VERSION_DMA; + // reboot boot menu timeout + self.fw_cfg_add_file("etc/boot-fail-wait", (5 as u32).as_bytes().to_vec()); + + let mut version = 0; + if self.dma_enabled { + version = FW_CFG_VERSION | FW_CFG_VERSION_DMA; + } self.fw_cfg_add_entry_data(FWCfgEntryType::FWCfgId as u16, version.as_bytes().to_vec()); } } @@ -706,7 +727,12 @@ impl DeviceOps for FWCfgMem { } }
- BigEndian::write_u64(data, value); + match data.len() { + 2 => BigEndian::write_u16(data, value as u16), + 4 => BigEndian::write_u32(data, value as u32), + 8 => BigEndian::write_u64(data, value as u64), + _ => info!("Make sure read size right ?"), + } true }
@@ -715,9 +741,13 @@ impl DeviceOps for FWCfgMem { let mut value = match size { 2 => BigEndian::read_u16(data) as u64, 4 => BigEndian::read_u32(data) as u64, + 8 => BigEndian::read_u64(data) as u64, _ => 0, }; - info!("FWCfgMem write value 0x{:x} size 0x{:x}", value, size); + info!( + "FWCfgMem write value 0x{:x} offset 0x{:x} size 0x{:x}", + value, offset, size + ); match offset { 0..=7 => { self.fw_cfg_data_mem_write(offset, value, size); @@ -756,9 +786,9 @@ impl FWCfgIO { FWCfgIO { fwcfg: FWCfgCommon { file_slots: FW_CFG_FILE_SLOTS_DFLT as u16, - local_entries: Vec::new(), - entries: Vec::new(), - files: Vec::new(), + local_entries: BTreeMap::new(), + entries: BTreeMap::new(), + files: BTreeMap::new(), cur_entry: 0, cur_offset: 0, dma_enabled: true, diff --git a/device_model/src/legacy/pl011.rs b/device_model/src/legacy/pl011.rs index 9f435c7..4634227 100644 --- a/device_model/src/legacy/pl011.rs +++ b/device_model/src/legacy/pl011.rs @@ -118,7 +118,7 @@ impl PL011 { /// * fail to write EventFd. /// * fail to get an interrupt event fd. fn interrupt(&self) -> Result<()> { - info!("interrupt once"); + debug!("interrupt once"); match &self.interrupt_evt { Some(evt) => evt.write(1).chain_err(|| "Failed to write fd")?, None => bail!("Failed to get an interrupt event fd"), @@ -322,7 +322,7 @@ impl DeviceOps for PL011 { } self.lcr = value; self.read_trigger = 1; - info!("write lcr 0x{:x}", self.lcr); + debug!("write lcr 0x{:x}", self.lcr); } 12 => { // UARTCR self.cr = value; @@ -411,9 +411,9 @@ impl EventNotifierHelper for PL011 { Box::new(move |_, _| { let mut out = [0_u8; 64]; if let Ok(count) = std::io::stdin().lock().read_raw(&mut out) { - info!("read {} bytes from stdin", count); + debug!("read {} bytes from stdin", count); let _ = pl011.lock().unwrap().receive(&out[..count]); - info!("receive done here"); + debug!("receive done here"); } None });