diff --git a/kernel/src/device/clock/bcm2835_aux.rs b/kernel/src/device/clock/bcm2835_aux.rs index 92bf2086..10c9a4af 100644 --- a/kernel/src/device/clock/bcm2835_aux.rs +++ b/kernel/src/device/clock/bcm2835_aux.rs @@ -3,10 +3,13 @@ use aarch64_cpu::registers::ReadWriteable; use abi::error::Error; use alloc::sync::Arc; use device_api::{ - clock::ClockController, + clock::{ClockController, ClockHandle}, device::{Device, DeviceInitContext}, }; -use device_tree::driver::{device_tree_driver, Node, ProbeContext}; +use device_tree::{ + driver::{device_tree_driver, DeviceTreeClockController, Node, ProbeContext}, + DeviceTreePropertyRead, TProp, +}; use libk_mm::{address::PhysicalAddress, device::DeviceMemoryIo}; use libk_util::{sync::IrqSafeSpinlock, OneTimeInit}; use tock_registers::{ @@ -37,14 +40,25 @@ struct Bcm2835Aux { regs: OneTimeInit>>, } +impl Bcm2835Aux { + fn ensure_regs(&self) -> Result<&IrqSafeSpinlock>, Error> { + let base = self.base; + self.regs.or_try_init_with(move || unsafe { + DeviceMemoryIo::map(base, Default::default()).map(IrqSafeSpinlock::new) + }) + } +} + impl ClockController for Bcm2835Aux { fn enable_clock(&self, clock: Option) -> Result<(), Error> { - let regs = self.regs.try_get().ok_or(Error::DoesNotExist)?.lock(); + let regs = self.ensure_regs()?; + let regs = regs.lock(); match clock { Some(0) => { regs.AUX_ENABLES.modify(AUX_ENABLES::MU_ENABLE::SET); Ok(()) } + None => loop {}, _ => Err(Error::InvalidArgument), } } @@ -54,10 +68,21 @@ impl ClockController for Bcm2835Aux { } } +impl DeviceTreeClockController for Bcm2835Aux { + fn map_clock(self: Arc, property: &TProp, offset: usize) -> Option<(ClockHandle, usize)> { + let clock = property.read_cell(offset, 1)? as u32; + Some(( + ClockHandle { + parent: self.clone(), + clock: Some(clock), + }, + 1, + )) + } +} + impl Device for Bcm2835Aux { unsafe fn init(self: Arc, _cx: DeviceInitContext) -> Result<(), Error> { - let regs = DeviceMemoryIo::map(self.base, Default::default())?; - self.regs.init(IrqSafeSpinlock::new(regs)); Ok(()) } @@ -76,12 +101,15 @@ device_tree_driver! { driver: { fn probe(&self, node: &Arc, context: &mut ProbeContext) -> Option> { let base = node.map_base(context, 0)?; - - Some(Arc::new(Bcm2835Aux { + let aux = Arc::new(Bcm2835Aux { _node: node.clone(), base, regs: OneTimeInit::new(), - })) + }); + + node.make_clock_controller(aux.clone()); + + Some(aux) } } }