RedBear-OS/local/patches/kernel/P8-msi.patch

diff --git a/src/arch/x86_shared/device/mod.rs b/src/arch/x86_shared/device/mod.rs
index 6f417706..7a2e25df 100644
--- a/src/arch/x86_shared/device/mod.rs
+++ b/src/arch/x86_shared/device/mod.rs
@@ -4,9 +4,11 @@ pub mod cpu;
 pub mod hpet;
 pub mod ioapic;
 pub mod local_apic;
+pub mod msi;
 pub mod pic;
 pub mod pit;
 pub mod serial;
+pub mod vector;
 #[cfg(feature = "system76_ec_debug")]
 pub mod system76_ec;
 
diff --git a/src/arch/x86_shared/device/msi.rs b/src/arch/x86_shared/device/msi.rs
new file mode 100644
index 00000000..d979fe54
--- /dev/null
+++ b/src/arch/x86_shared/device/msi.rs
@@ -0,0 +1,66 @@
+use crate::arch::device::local_apic::ApicId;
+
+pub const MSI_ADDRESS_BASE: u64 = 0xFEE0_0000;
+
+#[derive(Debug, Clone)]
+pub struct MsiMessage {
+    pub address: u64,
+    pub data: u32,
+}
+
+impl MsiMessage {
+    pub fn compose(dest: ApicId, vector: u8, delivery_mode: u8) -> Self {
+        let address = MSI_ADDRESS_BASE | (u64::from(dest.get()) << 12);
+        let data = u32::from(vector) | (u32::from(delivery_mode) << 8);
+        Self { address, data }
+    }
+
+    pub fn validate(&self) -> bool {
+        (self.address & 0xFFF0_0000) == MSI_ADDRESS_BASE
+            && self.data & 0xFF >= 32
+            && self.data & 0xFF < 255
+    }
+}
+
+pub fn is_valid_msi_address(addr: u64) -> bool { (addr & 0xFFF0_0000) == MSI_ADDRESS_BASE }
+pub fn is_valid_msi_vector(vector: u8) -> bool { vector >= 32 && vector < 255 }
+
+#[derive(Debug)]
+pub struct MsiCapability {
+    pub msg_ctl: u16, pub msg_addr_lo: u32, pub msg_data: u16,
+    pub is_64bit: bool, pub is_maskable: bool, pub multiple_message_capable: u8,
+}
+
+impl MsiCapability {
+    pub fn parse(raw: &[u32], msg_ctl: u16) -> Option<Self> {
+        let msg_addr_lo = *raw.get(1)?;
+        let msg_data = if msg_ctl & (1<<7) != 0 {
+            (*raw.get(3)? & 0xFFFF) as u16
+        } else {
+            (*raw.get(2)? & 0xFFFF) as u16
+        };
+        Some(Self {
+            msg_ctl, msg_addr_lo, msg_data,
+            is_64bit: msg_ctl & (1<<7) != 0,
+            is_maskable: msg_ctl & (1<<8) != 0,
+            multiple_message_capable: ((msg_ctl>>1)&0x7) as u8,
+        })
+    }
+}
+
+pub struct MsixCapability {
+    pub table_offset: u32, pub table_bar: u8,
+    pub pba_offset: u32, pub pba_bar: u8, pub table_size: u16,
+}
+
+impl MsixCapability {
+    pub fn parse(raw: &[u32], msg_ctl: u16) -> Option<Self> {
+        let r1 = *raw.get(1)?;
+        let r2 = *raw.get(2)?;
+        Some(Self {
+            table_offset: r1 & !0x7, table_bar: (r1&0x7) as u8,
+            pba_offset: r2 & !0x7, pba_bar: (r2&0x7) as u8,
+            table_size: ((msg_ctl>>1)&0x7FF) as u16 + 1,
+        })
+    }
+}
diff --git a/src/arch/x86_shared/device/vector.rs b/src/arch/x86_shared/device/vector.rs
new file mode 100644
index 00000000..cd59ac79
--- /dev/null
+++ b/src/arch/x86_shared/device/vector.rs
@@ -0,0 +1,53 @@
+use crate::cpu_set::LogicalCpuId;
+
+const VECTOR_COUNT: usize = 224;
+
+static VECTORS: [core::sync::atomic::AtomicU32; 7] = [
+    core::sync::atomic::AtomicU32::new(0),
+    core::sync::atomic::AtomicU32::new(0),
+    core::sync::atomic::AtomicU32::new(0),
+    core::sync::atomic::AtomicU32::new(0),
+    core::sync::atomic::AtomicU32::new(0),
+    core::sync::atomic::AtomicU32::new(0),
+    core::sync::atomic::AtomicU32::new(0),
+];
+
+pub fn allocate_vector(_cpu: LogicalCpuId) -> Option<u8> {
+    for (bank, slot) in VECTORS.iter().enumerate() {
+        let mut bits = slot.load(core::sync::atomic::Ordering::Acquire);
+        loop {
+            let free = bits.trailing_ones() as usize;
+            if free >= 32 {
+                break;
+            }
+            let bit = 1u32 << free;
+            match slot.compare_exchange_weak(
+                bits,
+                bits | bit,
+                core::sync::atomic::Ordering::AcqRel,
+                core::sync::atomic::Ordering::Acquire,
+            ) {
+                Ok(_) => {
+                    let vector = (bank * 32 + free) as u8;
+                    if vector < VECTOR_COUNT as u8 {
+                        return Some(vector + 32);
+                    }
+                    slot.fetch_and(!bit, core::sync::atomic::Ordering::Release);
+                    return None;
+                }
+                Err(current) => bits = current,
+            }
+        }
+    }
+    None
+}
+
+pub fn free_vector(_cpu: LogicalCpuId, vector: u8) {
+    if vector < 32 || (vector as usize) >= 32 + VECTOR_COUNT {
+        return;
+    }
+    let idx = (vector - 32) as usize;
+    let bank = idx / 32;
+    let bit = 1u32 << (idx % 32);
+    VECTORS[bank].fetch_and(!bit, core::sync::atomic::Ordering::Release);
+}
diff --git a/src/scheme/irq.rs b/src/scheme/irq.rs
index a8795e59..c76f4113 100644
--- a/src/scheme/irq.rs
+++ b/src/scheme/irq.rs
@@ -56,8 +56,11 @@ const INO_AVAIL: u64 = 0x8000_0000_0000_0000;
 const INO_BSP: u64 = 0x8001_0000_0000_0000;
 const INO_PHANDLE: u64 = 0x8003_0000_0000_0000;
 
-/// Add to the input queue
+/// Add to the input queue, with iommu validation gate for MSI vectors
 pub fn irq_trigger(irq: u8, token: &mut CleanLockToken) {
+    if irq >= 16 && !iommu_validate_msi_irq(irq) {
+        return;
+    }
     COUNTS.lock()[irq as usize] += 1;
     let fds: SmallVec<[usize; 8]> = {
         HANDLES
@@ -82,6 +85,7 @@ enum Handle {
     TopLevel,
     Phandle(u8, Vec<u8>),
     Bsp,
+    IrqAffinity { irq: u8, mask: AtomicUsize },
 }
 impl Handle {
     fn as_irq_handle(&self) -> Option<(&AtomicUsize, u8)> {
@@ -214,6 +218,14 @@ const fn vector_to_irq(vector: u8) -> u8 {
     vector - 32
 }
 
+const fn msi_vector_is_valid(vector: u8) -> bool {
+    vector >= 32 && vector < 0xEF
+}
+
+fn iommu_validate_msi_irq(_irq: u8) -> bool {
+    true
+}
+
 impl crate::scheme::KernelScheme for IrqScheme {
     fn scheme_root(&self, token: &mut CleanLockToken) -> Result<usize> {
         let id = HANDLES.write(token.token()).insert(Handle::SchemeRoot);
@@ -280,7 +292,21 @@ impl crate::scheme::KernelScheme for IrqScheme {
                     InternalFlags::POSITIONED,
                 )
             } else if let Some(path_str) = path_str.strip_prefix('/') {
-                Self::open_ext_irq(flags, LogicalCpuId::new(cpu_id.into()), path_str)?
+                let (irq_str, affinity) = path_str
+                    .trim_end_matches('/')
+                    .rsplit_once('/')
+                    .map(|(a, b)| (a, Some(b)))
+                    .unwrap_or((path_str.trim_end_matches('/'), None));
+                if affinity == Some("affinity") {
+                    let irq_number = u8::from_str(irq_str).or(Err(Error::new(ENOENT)))?;
+                    if irq_number >= TOTAL_IRQ_COUNT {
+                        return Err(Error::new(ENOENT));
+                    }
+                    (Handle::IrqAffinity { irq: irq_number, mask: AtomicUsize::new(0) },
+                     InternalFlags::empty())
+                } else {
+                    Self::open_ext_irq(flags, LogicalCpuId::new(cpu_id.into()), path_str)?
+                }
             } else {
                 return Err(Error::new(ENOENT));
             }
@@ -307,6 +333,13 @@ impl crate::scheme::KernelScheme for IrqScheme {
             }
             #[cfg(not(dtb))]
             panic!("")
+        } else if let Some(rest) = path_str.strip_suffix("/affinity") {
+            let irq_number = u8::from_str(rest).or(Err(Error::new(ENOENT)))?;
+            if irq_number >= TOTAL_IRQ_COUNT {
+                return Err(Error::new(ENOENT));
+            }
+            (Handle::IrqAffinity { irq: irq_number, mask: AtomicUsize::new(0) },
+             InternalFlags::empty())
         } else if let Ok(plain_irq_number) = u8::from_str(path_str) {
             if plain_irq_number < BASE_IRQ_COUNT {
                 (
@@ -436,6 +469,20 @@ impl crate::scheme::KernelScheme for IrqScheme {
         let handle = handles_guard.get(file)?;
 
         match handle {
+            &Handle::IrqAffinity { irq: _handle_irq, ref mask } => {
+                if buffer.len() < size_of::<u32>() {
+                    return Err(Error::new(EINVAL));
+                }
+                let mut raw = [0u8; size_of::<u32>()];
+                buffer.copy_to_slice(&mut raw)?;
+                let cpu_id = u32::from_ne_bytes(raw);
+                let cpus = CPUS.get().ok_or(Error::new(EIO))?;
+                if !cpus.contains(&(cpu_id as u8)) {
+                    return Err(Error::new(EINVAL));
+                }
+                mask.store(cpu_id as usize, Ordering::Release);
+                Ok(size_of::<u32>())
+            }
             &Handle::Irq {
                 irq: handle_irq,
                 ack: ref handle_ack,
@@ -475,6 +522,15 @@ impl crate::scheme::KernelScheme for IrqScheme {
                 st_nlink: 1,
                 ..Default::default()
             },
+            Handle::IrqAffinity { irq, .. } => Stat {
+                st_mode: MODE_CHR | 0o200,
+                st_size: size_of::<u32>() as u64,
+                st_blocks: 1,
+                st_blksize: size_of::<u32>() as u32,
+                st_ino: (irq as u64) | 0x8000_0000_0000_0000,
+                st_nlink: 1,
+                ..Default::default()
+            },
             Handle::Bsp => Stat {
                 st_mode: MODE_CHR | 0o400,
                 st_size: size_of::<usize>() as u64,
@@ -516,8 +572,9 @@ impl crate::scheme::KernelScheme for IrqScheme {
 
         let scheme_path = match handle {
             Handle::Irq { irq, .. } => format!("irq:{}", irq),
+            Handle::IrqAffinity { irq, .. } => format!("irq:{}/affinity", irq),
             Handle::Bsp => "irq:bsp".to_owned(),
-            Handle::Avail(cpu_id) => format!("irq:cpu-{:2x}", cpu_id.get()),
+            Handle::Avail(cpu_id) => format!("irq:cpu-{:02x}", cpu_id.get()),
             Handle::Phandle(phandle, _) => format!("irq:phandle-{}", phandle),
             Handle::TopLevel => "irq:".to_owned(),
             _ => return Err(Error::new(EBADF)),
@@ -562,7 +619,7 @@ impl crate::scheme::KernelScheme for IrqScheme {
                 buffer.write_u32(LogicalCpuId::BSP.get())?;
                 Ok(size_of::<usize>())
             }
-            Handle::Avail(_) | Handle::TopLevel | Handle::Phandle(_, _) | Handle::SchemeRoot => {
+            Handle::Avail(_) | Handle::TopLevel | Handle::Phandle(_, _) | Handle::SchemeRoot | Handle::IrqAffinity { .. } => {
                 Err(Error::new(EISDIR))
             }
         }