bcm283x: fix bcm283x_aux init/traits

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<IrqSafeSpinlock<DeviceMemoryIo<'static, Regs>>>,
 }
 
+impl Bcm2835Aux {
+    fn ensure_regs(&self) -> Result<&IrqSafeSpinlock<DeviceMemoryIo<'static, Regs>>, 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<u32>) -> 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<Self>, 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<Self>, _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<Node>, context: &mut ProbeContext) -> Option<Arc<dyn Device>> {
             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)
         }
     }
 }