From 7db6667e6bbc6287998620acd2ac8dd81aa2f6f1 Mon Sep 17 00:00:00 2001 From: Andrey Turkin Date: Sat, 16 Nov 2024 08:32:46 +0300 Subject: [PATCH] Better parsing of IRQ specifications in DTB Fixes Raspberry 3B+ DTB parsing (as generated by Qemu) --- src/arch/aarch64/device/generic_timer.rs | 17 +++--- src/arch/aarch64/device/irqchip/gic.rs | 13 +++-- src/arch/aarch64/device/irqchip/gicv3.rs | 13 +++-- .../aarch64/device/irqchip/irq_bcm2835.rs | 32 +++++------ .../aarch64/device/irqchip/irq_bcm2836.rs | 10 ++-- src/arch/aarch64/device/irqchip/null.rs | 4 +- src/arch/aarch64/device/serial.rs | 13 ++--- src/arch/riscv64/device/irqchip/clint_sbi.rs | 10 ++-- src/arch/riscv64/device/irqchip/hlic.rs | 13 +++-- src/arch/riscv64/device/irqchip/plic.rs | 15 +++--- src/arch/riscv64/device/serial.rs | 18 ++----- src/dtb/irqchip.rs | 53 +++++++++++++------ src/dtb/mod.rs | 31 ++++++++++- src/main.rs | 2 +- src/scheme/irq.rs | 16 ++---- 15 files changed, 139 insertions(+), 121 deletions(-) diff --git a/src/arch/aarch64/device/generic_timer.rs b/src/arch/aarch64/device/generic_timer.rs index 2250d91d0c..c1a4fff6bf 100644 --- a/src/arch/aarch64/device/generic_timer.rs +++ b/src/arch/aarch64/device/generic_timer.rs @@ -6,11 +6,13 @@ use crate::{ context, context::timeout, device::cpu::registers::control_regs, - dtb::irqchip::{register_irq, InterruptHandler, IRQ_CHIP}, + dtb::{ + get_interrupt, + irqchip::{register_irq, InterruptHandler, IRQ_CHIP}, + }, interrupt::irq::trigger, time, }; -use byteorder::{ByteOrder, BE}; use fdt::Fdt; bitflags! { @@ -28,19 +30,12 @@ pub unsafe fn init(fdt: &Fdt) { }; timer.init(); if let Some(node) = fdt.find_compatible(&["arm,armv7-timer"]) { - let interrupts = node.property("interrupts").unwrap(); - let irq = interrupts - .value - .array_chunks::<4>() - .map(|f| BE::read_u32(f)) - .skip(3) - .next_chunk::<3>() - .unwrap(); + let irq = get_interrupt(fdt, &node, 0).unwrap(); if let Some(ic_idx) = ic_for_chip(&fdt, &node) { //PHYS_NONSECURE_PPI only let virq = IRQ_CHIP.irq_chip_list.chips[ic_idx] .ic - .irq_xlate(&irq) + .irq_xlate(irq) .unwrap(); info!("generic_timer virq = {}", virq); register_irq(virq as u32, Box::new(timer)); diff --git a/src/arch/aarch64/device/irqchip/gic.rs b/src/arch/aarch64/device/irqchip/gic.rs index a380d53e5a..1507360d42 100644 --- a/src/arch/aarch64/device/irqchip/gic.rs +++ b/src/arch/aarch64/device/irqchip/gic.rs @@ -1,7 +1,7 @@ use super::InterruptController; use crate::dtb::{ get_mmio_address, - irqchip::{InterruptHandler, IrqDesc}, + irqchip::{InterruptHandler, IrqCell, IrqDesc}, }; use core::ptr::{read_volatile, write_volatile}; use fdt::{node::FdtNode, Fdt}; @@ -132,14 +132,13 @@ impl InterruptController for GenericInterruptController { fn irq_disable(&mut self, irq_num: u32) { unsafe { self.gic_dist_if.irq_disable(irq_num) } } - fn irq_xlate(&self, irq_data: &[u32; 3]) -> Result { - let mut off = match irq_data[0] { - 0 => irq_data[1] as usize + 32, //SPI - 1 => irq_data[1] as usize + 16, //PPI, + fn irq_xlate(&self, irq_data: IrqCell) -> Result { + let off = match irq_data { + IrqCell::L3(0, irq, _flags) => irq as usize + 32, // SPI + IrqCell::L3(1, irq, _flags) => irq as usize + 16, // PPI _ => return Err(Error::new(EINVAL)), }; - off += self.irq_range.0; - return Ok(off); + return Ok(off + self.irq_range.0); } fn irq_to_virq(&self, hwirq: u32) -> Option { if hwirq >= self.gic_dist_if.nirqs { diff --git a/src/arch/aarch64/device/irqchip/gicv3.rs b/src/arch/aarch64/device/irqchip/gicv3.rs index 13d36527b6..166e9709ee 100644 --- a/src/arch/aarch64/device/irqchip/gicv3.rs +++ b/src/arch/aarch64/device/irqchip/gicv3.rs @@ -5,7 +5,7 @@ use fdt::{node::NodeProperty, Fdt}; use super::{gic::GicDistIf, InterruptController}; use crate::dtb::{ get_mmio_address, - irqchip::{InterruptHandler, IrqDesc}, + irqchip::{InterruptHandler, IrqCell, IrqDesc}, }; use syscall::{ error::{Error, EINVAL}, @@ -127,14 +127,13 @@ impl InterruptController for GicV3 { fn irq_disable(&mut self, irq_num: u32) { unsafe { self.gic_dist_if.irq_disable(irq_num) } } - fn irq_xlate(&self, irq_data: &[u32; 3]) -> Result { - let mut off = match irq_data[0] { - 0 => irq_data[1] as usize + 32, //SPI - 1 => irq_data[1] as usize + 16, //PPI, + fn irq_xlate(&self, irq_data: IrqCell) -> Result { + let off = match irq_data { + IrqCell::L2(0, irq) => irq as usize + 32, // SPI + IrqCell::L2(1, irq) => irq as usize + 16, // PPI _ => return Err(Error::new(EINVAL)), }; - off += self.irq_range.0; - return Ok(off); + return Ok(off + self.irq_range.0); } fn irq_to_virq(&self, hwirq: u32) -> Option { if hwirq >= self.gic_dist_if.nirqs { diff --git a/src/arch/aarch64/device/irqchip/irq_bcm2835.rs b/src/arch/aarch64/device/irqchip/irq_bcm2835.rs index 64e0c64521..ee6cd32c61 100644 --- a/src/arch/aarch64/device/irqchip/irq_bcm2835.rs +++ b/src/arch/aarch64/device/irqchip/irq_bcm2835.rs @@ -1,12 +1,11 @@ -use byteorder::{ByteOrder, BE}; use core::ptr::{read_volatile, write_volatile}; use fdt::{node::FdtNode, Fdt}; use log::{debug, error, info}; use super::InterruptController; use crate::dtb::{ - get_mmio_address, - irqchip::{InterruptHandler, IrqDesc, IRQ_CHIP}, + get_interrupt, get_mmio_address, + irqchip::{InterruptHandler, IrqCell, IrqDesc, IRQ_CHIP}, }; use syscall::{ error::{Error, EINVAL}, @@ -81,18 +80,12 @@ impl Bcm2835ArmInterruptController { if let Some(interrupt_parent) = node.property("interrupt-parent") { let phandle = interrupt_parent.as_usize().unwrap() as u32; - let interrupts = node.property("interrupts").unwrap(); - let irq = interrupts - .value - .array_chunks::<4>() - .map(|f| BE::read_u32(f)) - .next_chunk::<3>() - .unwrap(); + let irq = get_interrupt(fdt, node, 0).unwrap(); let ic_idx = IRQ_CHIP.phandle_to_ic_idx(phandle).unwrap(); //PHYS_NONSECURE_PPI only let virq = IRQ_CHIP.irq_chip_list.chips[ic_idx] .ic - .irq_xlate(&irq) + .irq_xlate(irq) .unwrap(); info!("bcm2835arm_ctrl virq = {}", virq); ret_virq = Some(virq); @@ -252,14 +245,17 @@ impl InterruptController for Bcm2835ArmInterruptController { _ => return, } } - fn irq_xlate(&self, irq_data: &[u32; 3]) -> Result { + fn irq_xlate(&self, irq_data: IrqCell) -> Result { //assert interrupt-cells == 0x2 - let bank = irq_data[0] as usize; - let irq = irq_data[1] as usize; - //TODO: check bank && irq - let hwirq = bank << 5 | irq; - let off = hwirq + self.irq_range.0; - return Ok(off); + match irq_data { + IrqCell::L2(bank, irq) => { + //TODO: check bank && irq + let hwirq = (bank as usize) << 5 | (irq as usize); + let off = hwirq + self.irq_range.0; + Ok(off) + } + _ => Err(Error::new(EINVAL)), + } } fn irq_to_virq(&self, hwirq: u32) -> Option { diff --git a/src/arch/aarch64/device/irqchip/irq_bcm2836.rs b/src/arch/aarch64/device/irqchip/irq_bcm2836.rs index 545ce57757..3e418dc624 100644 --- a/src/arch/aarch64/device/irqchip/irq_bcm2836.rs +++ b/src/arch/aarch64/device/irqchip/irq_bcm2836.rs @@ -1,7 +1,7 @@ use super::InterruptController; use crate::dtb::{ get_mmio_address, - irqchip::{InterruptHandler, IrqDesc}, + irqchip::{InterruptHandler, IrqCell, IrqDesc}, }; use core::{ arch::asm, @@ -198,10 +198,12 @@ impl InterruptController for Bcm2836ArmInterruptController { } } } - fn irq_xlate(&self, irq_data: &[u32; 3]) -> Result { + fn irq_xlate(&self, irq_data: IrqCell) -> Result { //assert interrupt-cells == 0x2 - let off = irq_data[0] as usize + self.irq_range.0; - return Ok(off); + match irq_data { + IrqCell::L2(irq, _) => Ok(irq as usize + self.irq_range.0), + _ => Err(Error::new(EINVAL)), + } } fn irq_to_virq(&self, hwirq: u32) -> Option { if hwirq > LOCAL_IRQ_LAST { diff --git a/src/arch/aarch64/device/irqchip/null.rs b/src/arch/aarch64/device/irqchip/null.rs index d0c72bdd56..706e18752a 100644 --- a/src/arch/aarch64/device/irqchip/null.rs +++ b/src/arch/aarch64/device/irqchip/null.rs @@ -5,7 +5,7 @@ use syscall::{ }; use super::InterruptController; -use crate::dtb::irqchip::{InterruptHandler, IrqDesc}; +use crate::dtb::irqchip::{InterruptHandler, IrqCell, IrqDesc}; pub struct Null; @@ -29,7 +29,7 @@ impl InterruptController for Null { fn irq_eoi(&mut self, _irq_num: u32) {} fn irq_enable(&mut self, _irq_num: u32) {} fn irq_disable(&mut self, _irq_num: u32) {} - fn irq_xlate(&self, _irq_data: &[u32; 3]) -> Result { + fn irq_xlate(&self, _irq_data: IrqCell) -> Result { Err(Error::new(EINVAL)) } fn irq_to_virq(&self, _hwirq: u32) -> Option { diff --git a/src/arch/aarch64/device/serial.rs b/src/arch/aarch64/device/serial.rs index e507b68157..a81287f0fb 100644 --- a/src/arch/aarch64/device/serial.rs +++ b/src/arch/aarch64/device/serial.rs @@ -6,13 +6,12 @@ use crate::{ device::uart_pl011, devices::uart_16550, dtb::{ - diag_uart_range, + diag_uart_range, get_interrupt, irqchip::{register_irq, InterruptHandler, IRQ_CHIP}, }, interrupt::irq::trigger, scheme::debug::{debug_input, debug_notify}, }; -use byteorder::{ByteOrder, BE}; use fdt::Fdt; use log::{error, info}; use syscall::Mmio; @@ -120,17 +119,11 @@ pub unsafe fn init_early(dtb: &Fdt) { pub unsafe fn init(fdt: &Fdt) { //TODO: find actual serial device, not just any PL011 if let Some(node) = fdt.find_compatible(&["arm,pl011"]) { - let interrupts = node.property("interrupts").unwrap(); - let irq = interrupts - .value - .array_chunks::<4>() - .map(|f| BE::read_u32(f)) - .next_chunk::<3>() - .unwrap(); + let irq = get_interrupt(fdt, &node, 0).unwrap(); if let Some(ic_idx) = ic_for_chip(&fdt, &node) { let virq = IRQ_CHIP.irq_chip_list.chips[ic_idx] .ic - .irq_xlate(&irq) + .irq_xlate(irq) .unwrap(); info!("serial_port virq = {}", virq); register_irq(virq as u32, Box::new(Com1Irq {})); diff --git a/src/arch/riscv64/device/irqchip/clint_sbi.rs b/src/arch/riscv64/device/irqchip/clint_sbi.rs index c0471053d8..57aab226e5 100644 --- a/src/arch/riscv64/device/irqchip/clint_sbi.rs +++ b/src/arch/riscv64/device/irqchip/clint_sbi.rs @@ -1,7 +1,7 @@ use crate::{ context, context::timeout, - dtb::irqchip::{register_irq, InterruptHandler, IRQ_CHIP}, + dtb::irqchip::{register_irq, InterruptHandler, IrqCell, IRQ_CHIP}, }; use alloc::{boxed::Box, vec::Vec}; use byteorder::{ByteOrder, BE}; @@ -91,16 +91,16 @@ impl Clint { // FIXME dirty hack map M-mode interrupts (handled by SBI) to S-mode interrupts we get from SBI // Why aren't S-mode interrupts in the DTB already? - let irq0 = map_interrupt(irq0); - let irq1 = map_interrupt(irq1); + let irq0 = IrqCell::L1(map_interrupt(irq0)); + let irq1 = IrqCell::L1(map_interrupt(irq1)); let virq0 = hlic .ic - .irq_xlate(&[irq0, 0, 0]) + .irq_xlate(irq0) .expect("Couldn't get virq 0 from HLIC"); let virq1 = hlic .ic - .irq_xlate(&[irq1, 0, 0]) + .irq_xlate(irq1) .expect("Couldn't get virq 1 from HLIC"); register_irq(virq0 as u32, Box::new(ClintConnector { hart_id, irq: 0 })); register_irq(virq1 as u32, Box::new(ClintConnector { hart_id, irq: 1 })); diff --git a/src/arch/riscv64/device/irqchip/hlic.rs b/src/arch/riscv64/device/irqchip/hlic.rs index 454b5049a3..68021065c9 100644 --- a/src/arch/riscv64/device/irqchip/hlic.rs +++ b/src/arch/riscv64/device/irqchip/hlic.rs @@ -1,8 +1,8 @@ -use crate::dtb::irqchip::{InterruptController, InterruptHandler, IrqDesc, IRQ_CHIP}; +use crate::dtb::irqchip::{InterruptController, InterruptHandler, IrqCell, IrqDesc, IRQ_CHIP}; use alloc::vec::Vec; use core::arch::asm; use fdt::{node::NodeProperty, Fdt}; -use syscall::{Error, ENOENT}; +use syscall::{Error, EINVAL}; // This is a hart-local interrupt controller, a root of irqchip tree // An example DTS: @@ -137,11 +137,10 @@ impl InterruptController for Hlic { // Not bothering with this, all interrupts are enabled at all times } - fn irq_xlate(&self, irq_data: &[u32; 3]) -> syscall::Result { - let irq = irq_data[0]; - match irq { - 0..=0xF => Ok(self.virq_base + irq as usize), - _ => Err(Error::new(ENOENT)), + fn irq_xlate(&self, irq_data: IrqCell) -> syscall::Result { + match irq_data { + IrqCell::L1(irq) if irq <= 0xF => Ok(self.virq_base + irq as usize), + _ => Err(Error::new(EINVAL)), } } diff --git a/src/arch/riscv64/device/irqchip/plic.rs b/src/arch/riscv64/device/irqchip/plic.rs index 4c4b576069..242e392a8b 100644 --- a/src/arch/riscv64/device/irqchip/plic.rs +++ b/src/arch/riscv64/device/irqchip/plic.rs @@ -2,13 +2,13 @@ use crate::{ arch::{device::irqchip::hlic, start::BOOT_HART_ID}, dtb::{ get_mmio_address, - irqchip::{InterruptController, InterruptHandler, IrqDesc, IRQ_CHIP}, + irqchip::{InterruptController, InterruptHandler, IrqCell, IrqDesc, IRQ_CHIP}, }, }; use core::{mem, num::NonZero, sync::atomic::Ordering}; use fdt::Fdt; use log::{error, info}; -use syscall::{Error, Io, Mmio, ENODEV}; +use syscall::{Error, Io, Mmio, EINVAL}; #[repr(packed(4))] #[repr(C)] @@ -147,7 +147,7 @@ impl InterruptController for Plic { self.context = desc .parents .iter() - .position(|x| x.parent_interrupt[0] != u32::MAX && x.parent == hlic_ic_idx) + .position(|x| x.parent_interrupt.is_some() && x.parent == hlic_ic_idx) .unwrap(); info!("PLIC: using context {}", self.context); @@ -181,11 +181,10 @@ impl InterruptController for Plic { regs.enable(self.context, NonZero::new(irq_num as usize).unwrap(), false); } - fn irq_xlate(&self, irq_data: &[u32; 3]) -> syscall::Result { - if (irq_data[0] as usize) < self.ndev { - Ok(self.virq_base + irq_data[0] as usize) - } else { - Err(Error::new(ENODEV)) + fn irq_xlate(&self, irq_data: IrqCell) -> syscall::Result { + match irq_data { + IrqCell::L1(irq) => Ok(self.virq_base + irq as usize), + _ => Err(Error::new(EINVAL)), } } diff --git a/src/arch/riscv64/device/serial.rs b/src/arch/riscv64/device/serial.rs index 155145d723..b74e1a5094 100644 --- a/src/arch/riscv64/device/serial.rs +++ b/src/arch/riscv64/device/serial.rs @@ -1,5 +1,4 @@ use alloc::boxed::Box; -use byteorder::{ByteOrder, BE}; use fdt::Fdt; use log::info; use spin::Mutex; @@ -8,7 +7,7 @@ use syscall::Mmio; use crate::{ devices::uart_16550, dtb::{ - diag_uart_range, + diag_uart_range, get_interrupt, interrupt_parent, irqchip::{register_irq, InterruptHandler, IRQ_CHIP}, }, scheme::{ @@ -74,23 +73,14 @@ pub unsafe fn init_early(dtb: &Fdt) { pub unsafe fn init(fdt: &Fdt) -> Option<()> { if let Some(node) = fdt.find_compatible(&["ns16550a"]) { - let interrupts = node.property("interrupts").unwrap(); - let mut intr_data: [u32; 3] = [0, 0, 0]; - for (idx, chunk) in interrupts.value.chunks(4).enumerate() { - if idx >= intr_data.len() { - break; - } - let val = BE::read_u32(chunk); - intr_data[idx] = val; - } - - let interrupt_parent = node.interrupt_parent()?; + let intr = get_interrupt(fdt, &node, 0).unwrap(); + let interrupt_parent = interrupt_parent(fdt, &node)?; let phandle = interrupt_parent.property("phandle")?.as_usize()? as u32; let ic_idx = IRQ_CHIP.phandle_to_ic_idx(phandle)?; let virq = IRQ_CHIP.irq_chip_list.chips[ic_idx] .ic - .irq_xlate(&intr_data) + .irq_xlate(intr) .unwrap(); info!("serial_port virq = {}", virq); register_irq(virq as u32, Box::new(Com1Irq {})); diff --git a/src/dtb/irqchip.rs b/src/dtb/irqchip.rs index 9fe336ae6b..2a1f554da3 100644 --- a/src/dtb/irqchip.rs +++ b/src/dtb/irqchip.rs @@ -4,12 +4,20 @@ use alloc::{boxed::Box, vec::Vec}; use byteorder::{ByteOrder, BE}; use fdt::{node::NodeProperty, Fdt}; use log::{debug, error}; -use syscall::{Error, Result}; +use syscall::{Error, Result, EINVAL}; pub trait InterruptHandler { fn irq_handler(&mut self, irq: u32); } +#[derive(Debug, Copy, Clone)] +#[allow(dead_code)] +pub enum IrqCell { + L1(u32), + L2(u32, u32), + L3(u32, u32, u32), +} + pub trait InterruptController: InterruptHandler { fn irq_init( &mut self, @@ -23,14 +31,14 @@ pub trait InterruptController: InterruptHandler { fn irq_enable(&mut self, irq_num: u32); #[allow(unused)] fn irq_disable(&mut self, irq_num: u32); - fn irq_xlate(&self, irq_data: &[u32; 3]) -> Result; + fn irq_xlate(&self, irq_data: IrqCell) -> Result; fn irq_to_virq(&self, hwirq: u32) -> Option; } pub struct IrqConnection { pub parent_phandle: u32, pub parent: usize, // parent idx in chiplist - pub parent_interrupt: [u32; 3], + pub parent_interrupt: Option, } pub struct IrqChipItem { @@ -79,27 +87,36 @@ impl IrqChipList { fn interrupt_address( iter: &mut impl Iterator, interrupt_cells: usize, - ) -> Option<[u32; 3]> { + ) -> Option { match interrupt_cells { - 1 if let Some(a) = iter.next() => Some([a, 0, 0]), - 2 if let Ok([a, b]) = iter.next_chunk() => Some([a, b, 0]), - 3 => iter.next_chunk::<3>().ok(), + 1 => Some(IrqCell::L1(iter.next()?)), + 2 if let Ok([a, b]) = iter.next_chunk() => Some(IrqCell::L2(a, b)), + 3 if let Ok([a, b, c]) = iter.next_chunk() => Some(IrqCell::L3(a, b, c)), _ => None, } } + fn gate_interrupt_address(addr: IrqCell) -> Option { + match addr { + IrqCell::L1(u32::MAX) + | IrqCell::L2(u32::MAX, _) + | IrqCell::L3(u32::MAX, _, _) => None, + _ => Some(addr), + } + } + if let Some(parent) = node.interrupt_parent() && let Some(intr_data) = node.property("interrupts") { // FIXME use interrupts() helper when fixed (see gh#12) - let parent_interrupt_cells = parent.interrupt_cells().unwrap(); + let mut intr_data = intr_data.value.chunks(4).map(|x| BE::read_u32(x)); let parent_phandle = parent .property("phandle") .and_then(NodeProperty::as_usize) .unwrap() as u32; + let parent_interrupt_cells = parent.interrupt_cells().unwrap(); debug!("interrupt-parent = 0x{:08x}", parent_phandle); debug!("interrupts begin:"); - let mut intr_data = intr_data.value.chunks(4).map(|x| BE::read_u32(x)); while let Some(parent_interrupt) = interrupt_address(&mut intr_data, parent_interrupt_cells) { @@ -107,7 +124,7 @@ impl IrqChipList { item.parents.push(IrqConnection { parent_phandle, parent: 0, - parent_interrupt, + parent_interrupt: gate_interrupt_address(parent_interrupt), }); } debug!("interrupts end"); @@ -126,7 +143,7 @@ impl IrqChipList { item.parents.push(IrqConnection { parent_phandle, parent: 0, - parent_interrupt, + parent_interrupt: gate_interrupt_address(parent_interrupt), }); } } @@ -201,15 +218,15 @@ impl IrqChipList { let cur_chip = &self.chips[cur_idx]; for connection in &cur_chip.parents { debug_assert!(queue[0..queue_idx].contains(&connection.parent)); - if connection.parent_interrupt[0] != u32::MAX { + if let Some(parent_interrupt) = connection.parent_interrupt { let parent = &self.chips[connection.parent]; - if let Ok(virq) = parent.ic.irq_xlate(&connection.parent_interrupt) { + if let Ok(virq) = parent.ic.irq_xlate(parent_interrupt) { // assert is unused irq_desc[virq].basic.child_ic_idx = Some(cur_idx); } else { error!( "Cannot connect irq chip {} to parent irq {} : {:?}", - cur_chip.phandle, parent.phandle, connection.parent_interrupt + cur_chip.phandle, parent.phandle, parent_interrupt ); } } @@ -257,7 +274,13 @@ impl IrqChipCore { self.irq_chip_list.chips[ic_idx].ic.irq_to_virq(hwirq) } - pub fn irq_xlate(&self, ic_idx: usize, irq_data: &[u32; 3]) -> Result { + pub fn irq_xlate(&self, ic_idx: usize, irq_data: &[u32]) -> Result { + let irq_data = match irq_data.len() { + 1 => IrqCell::L1(irq_data[0]), + 2 => IrqCell::L2(irq_data[0], irq_data[1]), + 3 => IrqCell::L3(irq_data[0], irq_data[1], irq_data[2]), + _ => return Err(Error::new(EINVAL)), + }; self.irq_chip_list.chips[ic_idx].ic.irq_xlate(irq_data) } diff --git a/src/dtb/mod.rs b/src/dtb/mod.rs index 4a8fa1a81b..68a9dda536 100644 --- a/src/dtb/mod.rs +++ b/src/dtb/mod.rs @@ -1,6 +1,9 @@ pub mod irqchip; -use crate::startup::memory::{register_memory_region, BootloaderMemoryKind}; +use crate::{ + dtb::irqchip::IrqCell, + startup::memory::{register_memory_region, BootloaderMemoryKind}, +}; use alloc::vec::Vec; use byteorder::{ByteOrder, BE}; use core::slice; @@ -168,6 +171,32 @@ pub fn get_mmio_address(fdt: &Fdt, _device: &FdtNode, region: &MemoryRegion) -> Some(mapped_addr) } +pub fn interrupt_parent<'a>(fdt: &'a Fdt, node: &'a FdtNode) -> Option> { + // FIXME traverse device tree up + node.interrupt_parent() + .or_else(|| fdt.find_node("/soc").and_then(|soc| soc.interrupt_parent())) + .or_else(|| fdt.find_node("/").and_then(|node| node.interrupt_parent())) +} + +pub fn get_interrupt(fdt: &Fdt, node: &FdtNode, idx: usize) -> Option { + let interrupts = node.property("interrupts").unwrap(); + let parent_interrupt_cells = interrupt_parent(fdt, node) + .unwrap() + .interrupt_cells() + .unwrap(); + let mut intr = interrupts + .value + .array_chunks::<4>() + .map(|f| BE::read_u32(f)) + .skip(parent_interrupt_cells * idx); + match parent_interrupt_cells { + 1 => Some(IrqCell::L1(intr.next()?)), + 2 if let Ok([a, b]) = intr.next_chunk() => Some(IrqCell::L2(a, b)), + 3 if let Ok([a, b, c]) = intr.next_chunk() => Some(IrqCell::L3(a, b, c)), + _ => None, + } +} + pub fn diag_uart_range<'a>(dtb: &'a Fdt) -> Option<(usize, usize, bool, bool, &'a str)> { let stdout_path = dtb.chosen().stdout()?; let uart_node = stdout_path.node(); diff --git a/src/main.rs b/src/main.rs index f256eff2d3..a42ec1cf3f 100644 --- a/src/main.rs +++ b/src/main.rs @@ -57,7 +57,7 @@ #![feature(option_get_or_insert_default)] #![feature(array_chunks)] #![feature(if_let_guard)] - +#![feature(iterator_try_collect)] #[macro_use] extern crate alloc; diff --git a/src/scheme/irq.rs b/src/scheme/irq.rs index af48f6add0..49a179d280 100644 --- a/src/scheme/irq.rs +++ b/src/scheme/irq.rs @@ -165,16 +165,10 @@ impl IrqScheme { phandle: usize, path_str: &str, ) -> Result<(Handle, InternalFlags)> { - let mut path_iter = path_str.split(','); - let addr = path_iter.next_chunk::<3>().or(Err(Error::new(ENOENT)))?; - if path_iter.next().is_some() { - return Err(Error::new(ENOENT)); - } - let addr = [ - u32::from_str(addr[0]).or(Err(Error::new(ENOENT)))?, - u32::from_str(addr[1]).or(Err(Error::new(ENOENT)))?, - u32::from_str(addr[2]).or(Err(Error::new(ENOENT)))?, - ]; + let addr: Vec = path_str + .split(',') + .map(|x| u32::from_str(x).or(Err(Error::new(ENOENT)))) + .try_collect()?; let ic_idx = IRQ_CHIP .phandle_to_ic_idx(phandle as u32) .ok_or(Error::new(ENOENT))?; @@ -183,7 +177,7 @@ impl IrqScheme { return Err(Error::new(EINVAL)); } let irq_number = IRQ_CHIP - .irq_xlate(ic_idx, &addr) + .irq_xlate(ic_idx, addr.as_slice()) .or(Err(Error::new(ENOENT)))?; log::debug!("open_phandle_irq virq={}", irq_number); if flags & O_STAT == 0 {