jh7110: improve clocks, add generic syscon driver

kernel/lib/device-api/src/bus.rs

@@ -1,7 +1,10 @@
 //! Bus device interfaces
 use core::ops::Range;
 
-use crate::device::Device;
+use alloc::sync::Arc;
+use yggdrasil_abi::error::Error;
+
+use crate::{device::Device, dma::DmaAllocator};
 
 /// Bus device which provides an interconnect between two or more address spaces.
 ///
@@ -11,4 +14,9 @@ pub trait Bus: Device {
     ///
     /// NOTE: nested busses with bus1 -> bus0 -> host translation schemes are not yet supported.
     fn map_range(&self, bus_range: Range<u64>) -> Option<Range<u64>>;
+
+    /// TODO: document
+    fn create_dma_allocator(self: Arc<Self>) -> Result<Arc<dyn DmaAllocator>, Error> {
+        Err(Error::NotImplemented)
+    }
 }

kernel/lib/device-tree/src/driver/mod.rs

@@ -6,6 +6,7 @@ use yggdrasil_abi::error::Error;
 mod controller;
 mod macros;
 mod registry;
+mod syscon;
 mod traits;
 mod tree;
 
@@ -14,6 +15,7 @@ pub mod util;
 pub use controller::{map_interrupt, map_interrupt_at};
 pub use macros::device_tree_driver;
 pub use registry::{lookup_phandle, register_driver};
+pub use syscon::DeviceTreeSyscon;
 pub use traits::{
     DeviceTreeClockController, DeviceTreeInterruptController, DeviceTreeResetController, Driver,
     ProbeContext,

kernel/lib/device-tree/src/driver/syscon.rs

@@ -0,0 +1,113 @@
+//! System controller/regmap drivers
+use alloc::{string::String, sync::Arc};
+use device_api::device::Device;
+use libk::error::Error;
+use libk_mm::{address::PhysicalAddress, device::DeviceMemoryIoMut};
+use libk_util::sync::IrqSafeSpinlock;
+
+use crate::driver::{device_tree_driver, Node, ProbeContext};
+
+/// System controller/register map interface
+pub trait DeviceTreeSyscon: Device {
+    /// Reads a register
+    fn read_register(&self, address: usize) -> Result<u32, Error>;
+    /// Writes a register
+    fn write_register(&self, address: usize, val: u32) -> Result<(), Error>;
+    /// Modifies a register, clearing the `clear` bits and setting the `set` bits.
+    fn modify_register(&self, address: usize, clear: u32, set: u32) -> Result<(), Error>;
+}
+
+/// Simple syscon node driver
+pub struct SimpleSyscon {
+    regs: IrqSafeSpinlock<DeviceMemoryIoMut<'static, [u32]>>,
+    regs_len: usize,
+    name: String,
+}
+
+unsafe impl Send for SimpleSyscon {}
+unsafe impl Sync for SimpleSyscon {}
+
+impl SimpleSyscon {
+    fn validate_address(&self, address: usize) -> Result<(), Error> {
+        if address + 4 > self.regs_len || address % 4 != 0 {
+            log::error!("{}: invalid register access {:#x}", self.name, address);
+            Err(Error::InvalidArgument)
+        } else {
+            Ok(())
+        }
+    }
+}
+
+impl Device for SimpleSyscon {
+    fn display_name(&self) -> &str {
+        "syscon"
+    }
+}
+
+impl DeviceTreeSyscon for SimpleSyscon {
+    fn read_register(&self, address: usize) -> Result<u32, Error> {
+        self.validate_address(address)?;
+
+        let lock = self.regs.lock();
+        let ptr = &raw const lock[address / 4];
+
+        Ok(unsafe { ptr.read_volatile() })
+    }
+
+    fn write_register(&self, address: usize, val: u32) -> Result<(), Error> {
+        self.validate_address(address)?;
+
+        let mut lock = self.regs.lock();
+        let ptr = &raw mut lock[address / 4];
+
+        Ok(unsafe { ptr.write_volatile(val) })
+    }
+
+    fn modify_register(&self, address: usize, clear: u32, set: u32) -> Result<(), Error> {
+        self.validate_address(address)?;
+
+        let mut lock = self.regs.lock();
+        let ptr = &raw mut lock[address / 4];
+
+        unsafe {
+            let mut val = ptr.read_volatile();
+            val &= !clear;
+            val |= set;
+            ptr.write_volatile(val);
+        }
+
+        Ok(())
+    }
+}
+
+device_tree_driver! {
+    compatible: ["syscon"],
+    driver: {
+        fn probe(&self, node: &Arc<Node>, context: &mut ProbeContext) -> Option<Arc<dyn Device>> {
+            let range = node.map_range(context, 0)?;
+            let base = PhysicalAddress::from_u64(range.start);
+            let len = (range.end - range.start) as usize;
+            let name = if let Some(name) = node.name() {
+                name.into()
+            } else {
+                alloc::format!("syscon@{base:x}")
+            };
+
+            let regs = unsafe { DeviceMemoryIoMut::map_slice(base, len / 4, Default::default()) }
+                .inspect_err(|e| log::error!("{name}: map error: {e:?}"))
+                .ok()?;
+
+            log::info!("{name}: probed");
+
+            let syscon = Arc::new(SimpleSyscon {
+                regs: IrqSafeSpinlock::new(regs),
+                regs_len: len,
+                name,
+            });
+
+            node.make_system_controller(syscon.clone());
+
+            Some(syscon)
+        }
+    }
+}

kernel/lib/device-tree/src/driver/traits.rs

@@ -45,7 +45,8 @@ pub trait DeviceTreeResetController {
 
 /// Context passed to the driver's `probe` function
 pub struct ProbeContext {
-    pub(crate) bus: Option<Weak<dyn Bus>>,
+    /// Parent bus of the node being probed
+    pub bus: Option<Weak<dyn Bus>>,
     /// Can be used to set a specific initialization sequence for the device
     pub sequence: Option<InitSequence>,
 }

kernel/lib/device-tree/src/driver/tree.rs

@@ -17,7 +17,9 @@ use libk_mm::address::PhysicalAddress;
 use libk_util::OneTimeInit;
 use yggdrasil_abi::error::Error;
 
-use crate::{DeviceTree, DeviceTreeNodeExt, DeviceTreePropertyRead, TNode, TProp};
+use crate::{
+    driver::DeviceTreeSyscon, DeviceTree, DeviceTreeNodeExt, DeviceTreePropertyRead, TNode, TProp,
+};
 
 use super::{
     controller::{ClockIter, ResetIter},
@@ -52,6 +54,7 @@ pub struct Node {
     pub(crate) msi_controller: OneTimeInit<Arc<dyn MessageInterruptController>>,
     pub(crate) clock_controler: OneTimeInit<Arc<dyn DeviceTreeClockController>>,
     pub(crate) reset_controller: OneTimeInit<Arc<dyn DeviceTreeResetController>>,
+    pub(crate) system_controller: OneTimeInit<Arc<dyn DeviceTreeSyscon>>,
 }
 
 pub(crate) struct ProbedDevice {
@@ -204,6 +207,11 @@ impl Node {
         self.clock_controler.init(clkc);
     }
 
+    /// Informs the node of its capability as a system controller.
+    pub fn make_system_controller(&self, syscon: Arc<dyn DeviceTreeSyscon>) {
+        self.system_controller.init(syscon);
+    }
+
     /// Returns the device driver associated with this node, if any was probed.
     pub fn driver(&self) -> Option<&'static dyn Driver> {
         let probed = self.device.try_get()?.as_probed()?;
@@ -401,6 +409,11 @@ impl Node {
             .map(|e| e.clone().as_interrupt_controller())
     }
 
+    /// Attempts to get an system controller represented by this node, if any
+    pub fn as_system_controller(self: &Arc<Self>) -> Option<Arc<dyn DeviceTreeSyscon>> {
+        self.system_controller.try_get().cloned()
+    }
+
     /// Returns the `#address-cells` value of the node's parent bus
     pub fn bus_address_cells(&self) -> usize {
         self.bus_address_cells
@@ -440,6 +453,32 @@ impl Node {
     pub fn prop_str(&self, name: &str) -> Option<&'static str> {
         self.dt_node.prop_string(name)
     }
+
+    /// Returns `true` if the node contains `query` in its `compatible` property.
+    pub fn is_compatible(&self, query: &str) -> bool {
+        let Some(compatible) = self.compatible.as_ref() else {
+            return false;
+        };
+
+        compatible.as_str_list().any(|c| c == query)
+    }
+
+    /// Looks up the first node matching `compatible` query.
+    pub fn by_compatible(compatible: &str, ensure_probed: bool) -> Option<Arc<Self>> {
+        let node = walk_device_tree(|node| {
+            if node.is_compatible(compatible) {
+                Some(node.clone())
+            } else {
+                None
+            }
+        })?;
+
+        if ensure_probed {
+            node.probe()?;
+        }
+
+        Some(node)
+    }
 }
 
 impl fmt::Debug for Node {
@@ -491,6 +530,7 @@ fn unflatten_node(
         msi_controller: OneTimeInit::new(),
         clock_controler: OneTimeInit::new(),
         reset_controller: OneTimeInit::new(),
+        system_controller: OneTimeInit::new(),
     });
 
     if let Some(phandle) = phandle {

kernel/src/device/clock/jh7110_clocks.rs

@@ -6,12 +6,12 @@ use abi::error::Error;
 use alloc::sync::Arc;
 use device_api::{
     clock::{ClockController, ClockHandle, Hertz, ResetController, ResetHandle},
-    device::Device,
+    device::{Device, DeviceInitContext},
 };
 use device_tree::{
     driver::{
-        device_tree_driver, DeviceTreeClockController, DeviceTreeResetController, Node,
-        ProbeContext,
+        device_tree_driver, lookup_phandle, DeviceTreeClockController, DeviceTreeResetController,
+        DeviceTreeSyscon, Node, ProbeContext,
     },
     DeviceTreePropertyRead, TProp,
 };
@@ -33,6 +33,11 @@ const SYSCRG_AHB0: usize = 0x024 / 4;
 const SYSCRG_AHB1: usize = 0x028 / 4;
 const SYSCRG_APB_BUS: usize = 0x02C / 4;
 const SYSCRG_APB0: usize = 0x030 / 4;
+// usb
+const SYSCRG_USB_125M: usize = 0x17C / 4;
+const SYSCRG_NOC_BUS_STG_AXI: usize = 0x180 / 4;
+// misc
+const SYSCRG_IOMUX_APB: usize = 0x1C0 / 4;
 // gmac1
 const SYSCRG_GMAC1_AHB: usize = 0x184 / 4;
 const SYSCRG_GMAC1_AXI: usize = 0x188 / 4;
@@ -107,6 +112,51 @@ const AONCRG_APB_BUS: usize = AONCRG_CLOCK_COUNT + 4;
 const AONCRG_GMAC0_GTXCLK: usize = AONCRG_CLOCK_COUNT + 5;
 const AONCRG_RTC_OSC: usize = AONCRG_CLOCK_COUNT + 6;
 
+// STGCRG clocks
+
+// hifi4
+const STGCRG_HIFI4_CLK_CORE: usize = 0x00 / 4;
+// usb
+const STGCRG_USB_APB: usize = 0x04 / 4;
+const STGCRG_USB_UTMI_APB: usize = 0x08 / 4;
+const STGCRG_USB_AXI: usize = 0x0C / 4;
+const STGCRG_USB_LPM: usize = 0x10 / 4;
+const STGCRG_USB_STB: usize = 0x14 / 4;
+const STGCRG_USB_APP_125: usize = 0x18 / 4;
+const STGCRG_USB_REF: usize = 0x1C / 4;
+const STGCRG_PCIE0_AXI_MST0: usize = 0x20 / 4;
+const STGCRG_PCIE0_APB: usize = 0x24 / 4;
+const STGCRG_PCIE0_TL: usize = 0x28 / 4;
+const STGCRG_PCIE1_AXI_MST0: usize = 0x2C / 4;
+const STGCRG_PCIE1_APB: usize = 0x30 / 4;
+const STGCRG_PCIE1_TL: usize = 0x34 / 4;
+const STGCRG_PCIE1_SLV_DEC: usize = 0x38 / 4;
+const STGCRG_SECURITY_HCLK: usize = 0x3C / 4;
+const STGCRG_SECURITY_MISC_AHB: usize = 0x40 / 4;
+const STGCRG_MTRX_GRP0: usize = 0x44 / 4;
+const STGCRG_MTRX_GRP0_BUS: usize = 0x48 / 4;
+const STGCRG_MTRX_GRP0_STG: usize = 0x4C / 4;
+const STGCRG_MTRX_GRP1: usize = 0x50 / 4;
+const STGCRG_MTRX_GRP1_BUS: usize = 0x54 / 4;
+const STGCRG_MTRX_GRP1_STG: usize = 0x58 / 4;
+const STGCRG_MTRX_GRP1_HIFI: usize = 0x5C / 4;
+const STGCRG_E24_RTC: usize = 0x60 / 4;
+const STGCRG_E24_CORE: usize = 0x64 / 4;
+const STGCRG_E24_DBG: usize = 0x68 / 4;
+const STGCRG_DMA1P_AXI: usize = 0x6C / 4;
+const STGCRG_DMA1P_AHB: usize = 0x70 / 4;
+
+const STGCRG_CLOCK_COUNT: usize = STGCRG_DMA1P_AHB + 1;
+
+const STGCRG_OSC: usize = STGCRG_CLOCK_COUNT;
+const STGCRG_HIFI4_CORE: usize = STGCRG_CLOCK_COUNT + 1;
+const STGCRG_STG_AXIAHB: usize = STGCRG_CLOCK_COUNT + 2;
+const STGCRG_USB_125M: usize = STGCRG_CLOCK_COUNT + 3;
+const STGCRG_CPU_BUS: usize = STGCRG_CLOCK_COUNT + 4;
+const STGCRG_HIFI4_AXI: usize = STGCRG_CLOCK_COUNT + 5;
+const STGCRG_NOCSTG_BUS: usize = STGCRG_CLOCK_COUNT + 6;
+const STGCRG_APB_BUS: usize = STGCRG_CLOCK_COUNT + 7;
+
 #[allow(unused)]
 enum ClockDef {
     Mux(&'static [usize]),
@@ -144,6 +194,11 @@ const SYSCRG_CLOCKS: &'static [Option<(&'static str, ClockDef)>] = &const {
     t[SYSCRG_AHB1] = Some(("clk_ahb1", Gate(SYSCRG_STG_AXIAHB)));
     t[SYSCRG_APB_BUS] = Some(("clk_apb_bus", Div(8, SYSCRG_STG_AXIAHB)));
     t[SYSCRG_APB0] = Some(("clk_apb0", Gate(SYSCRG_APB_BUS)));
+    // usb
+    t[SYSCRG_USB_125M] = Some(("clk_usb_125m", Div(15, SYSCRG_PLL0_OUT)));
+    t[SYSCRG_NOC_BUS_STG_AXI] = Some(("clk_noc_bus_stg_axi", Gate(SYSCRG_NOCSTG_BUS)));
+    // misc
+    t[SYSCRG_IOMUX_APB] = Some(("clk_iomux_apb", Gate(SYSCRG_APB_BUS)));
     // gmac1
     t[SYSCRG_GMAC1_AHB] = Some(("clk_gmac1_ahb", Gate(SYSCRG_AHB0)));
     t[SYSCRG_GMAC1_AXI] = Some(("clk_gmac1_axi", Gate(SYSCRG_STG_AXIAHB)));
@@ -209,6 +264,51 @@ const AONCRG_CLOCKS: &'static [Option<(&'static str, ClockDef)>] = &const {
     t
 };
 
+const STGCRG_CLOCKS: &'static [Option<(&'static str, ClockDef)>] = &const {
+    use ClockDef::*;
+
+    let mut t = [const { None }; STGCRG_CLOCK_COUNT];
+
+    // hifi4
+    t[STGCRG_HIFI4_CLK_CORE] = Some(("clk_hifi4_clk_core", Gate(STGCRG_HIFI4_CORE)));
+    // usb
+    t[STGCRG_USB_APB] = Some(("clk_usb_apb", Gate(STGCRG_APB_BUS)));
+    t[STGCRG_USB_UTMI_APB] = Some(("clk_usb_utmi_apb", Gate(STGCRG_APB_BUS)));
+    t[STGCRG_USB_AXI] = Some(("clk_usb_axi", Gate(STGCRG_STG_AXIAHB)));
+    t[STGCRG_USB_LPM] = Some(("clk_usb_lpm", GateDiv(2, STGCRG_OSC)));
+    t[STGCRG_USB_STB] = Some(("clk_usb_stb", GateDiv(4, STGCRG_OSC)));
+    t[STGCRG_USB_APP_125] = Some(("clk_usb_app_125", Gate(STGCRG_USB_125M)));
+    t[STGCRG_USB_REF] = Some(("clk_usb_ref", Div(2, STGCRG_OSC)));
+    // pcie
+    t[STGCRG_PCIE0_AXI_MST0] = Some(("clk_pcie0_axi_mst0", Gate(STGCRG_STG_AXIAHB)));
+    t[STGCRG_PCIE0_APB] = Some(("clk_pcie0_apb", Gate(STGCRG_APB_BUS)));
+    t[STGCRG_PCIE0_TL] = Some(("clk_pcie0_tl", Gate(STGCRG_STG_AXIAHB)));
+    t[STGCRG_PCIE1_AXI_MST0] = Some(("clk_pcie1_axi_mst0", Gate(STGCRG_STG_AXIAHB)));
+    t[STGCRG_PCIE1_APB] = Some(("clk_pcie1_apb", Gate(STGCRG_APB_BUS)));
+    t[STGCRG_PCIE1_TL] = Some(("clk_pcie1_tl", Gate(STGCRG_STG_AXIAHB)));
+    t[STGCRG_PCIE1_SLV_DEC] = Some(("clk_pcie1_slv_dec", Gate(STGCRG_STG_AXIAHB)));
+    // security
+    t[STGCRG_SECURITY_HCLK] = Some(("clk_security_hclk", Gate(STGCRG_STG_AXIAHB)));
+    t[STGCRG_SECURITY_MISC_AHB] = Some(("clk_security_misc_ahb", Gate(STGCRG_STG_AXIAHB)));
+    // mtrx
+    t[STGCRG_MTRX_GRP0] = Some(("clk_mtrx_grp0", Gate(STGCRG_CPU_BUS)));
+    t[STGCRG_MTRX_GRP0_BUS] = Some(("clk_mrtx_grp0_bus", Gate(STGCRG_NOCSTG_BUS)));
+    t[STGCRG_MTRX_GRP0_STG] = Some(("clk_mtrx_grp0_stg", Gate(STGCRG_STG_AXIAHB)));
+    t[STGCRG_MTRX_GRP1] = Some(("clk_mtrx_grp1", Gate(STGCRG_CPU_BUS)));
+    t[STGCRG_MTRX_GRP1_BUS] = Some(("clk_mtrx_grp1_bus", Gate(STGCRG_NOCSTG_BUS)));
+    t[STGCRG_MTRX_GRP1_STG] = Some(("clk_mtrx_grp1_stg", Gate(STGCRG_STG_AXIAHB)));
+    t[STGCRG_MTRX_GRP1_HIFI] = Some(("clk_mtrx_grp1_hifi", Gate(STGCRG_HIFI4_AXI)));
+    // e24
+    t[STGCRG_E24_RTC] = Some(("clk_e24_rtc", GateDiv(24, STGCRG_OSC)));
+    t[STGCRG_E24_CORE] = Some(("clk_e24_core", Gate(STGCRG_STG_AXIAHB)));
+    t[STGCRG_E24_DBG] = Some(("clk_e24_dbg", Gate(STGCRG_STG_AXIAHB)));
+    // dma1p
+    t[STGCRG_DMA1P_AXI] = Some(("clk_dma1p_axi", Gate(STGCRG_STG_AXIAHB)));
+    t[STGCRG_DMA1P_AHB] = Some(("clk_dma1p_ahb", Gate(STGCRG_STG_AXIAHB)));
+
+    t
+};
+
 struct Crg<C, P> {
     clock_mapping: IrqSafeSpinlock<DeviceMemoryIoMut<'static, [u32]>>,
     reset_mapping: IrqSafeSpinlock<DeviceMemoryIoMut<'static, [u32]>>,
@@ -216,8 +316,33 @@ struct Crg<C, P> {
     _pd: PhantomData<C>,
 }
 
-// TODO this is just a dummy, which is assumed to be always running
-struct Pll;
+struct Pll {
+    syscon: Arc<dyn DeviceTreeSyscon>,
+    clk_osc: ClockHandle,
+}
+
+struct PllDef {
+    fbdiv_offset: usize,
+    pd_offset: usize,
+    frac_offset: usize,
+    prediv_offset: usize,
+
+    dacpd_bit: u32,
+    dsmpd_bit: u32,
+
+    fbdiv_shift: usize,
+}
+
+#[derive(Debug)]
+struct PllRegs {
+    dacpd: bool,
+    dsmpd: bool,
+    fbdiv: u32,
+    #[allow(unused)]
+    frac: u32,
+    postdiv1: u32,
+    prediv: u32,
+}
 
 unsafe impl<C, P> Send for Crg<C, P> {}
 unsafe impl<C, P> Sync for Crg<C, P> {}
@@ -251,6 +376,10 @@ impl<C: ClockDefinition, P: Index<usize, Output = ClockHandle>> Crg<C, P> {
             })
     }
 
+    fn read_clock_reg(&self, index: usize) -> u32 {
+        self.clock_mapping.lock()[index]
+    }
+
     fn enable_clock_inner(&self, index: usize) -> Result<(), Error> {
         if index >= C::CLOCKS.len() {
             return self.parents[index - C::CLOCKS.len()].enable();
@@ -292,13 +421,16 @@ impl<C: ClockDefinition, P: Index<usize, Output = ClockHandle>> Crg<C, P> {
 
         let (_, clk_def) = self.clock(index)?;
 
+        let reg = self.read_clock_reg(index);
+        let reg_div = reg & 0xFFFFFF;
+
         let (parent, div) = match clk_def {
             &ClockDef::Gate(parent) | &ClockDef::Inv(parent) => (parent, 1),
-            &ClockDef::Div(div, parent) => (parent, div),
+            &ClockDef::Div(_max, parent) => (parent, reg_div),
             // TODO read actual parent for muxes
-            &ClockDef::GateDiv(div, parent) => (parent, div),
+            &ClockDef::GateDiv(_max, parent) => (parent, reg_div),
             ClockDef::GateMux(parents) | ClockDef::Mux(parents) => (parents[0], 1),
-            ClockDef::MuxDiv(div, parents) => (parents[0], *div),
+            ClockDef::MuxDiv(_max, parents) => (parents[0], reg_div),
         };
 
         let parent_rate = self.clock_rate_inner(parent)?;
@@ -393,7 +525,94 @@ impl<C: ClockDefinition + 'static, P: Index<usize, Output = ClockHandle> + 'stat
     }
 }
 
+impl Pll {
+    const PLLS: &[PllDef] = &[
+        PllDef {
+            pd_offset: 0x18,
+            fbdiv_offset: 0x1C,
+            frac_offset: 0x20,
+            prediv_offset: 0x24,
+
+            dacpd_bit: 1 << 24,
+            dsmpd_bit: 1 << 25,
+
+            fbdiv_shift: 0,
+        },
+        PllDef {
+            pd_offset: 0x24,
+            fbdiv_offset: 0x24,
+            frac_offset: 0x28,
+            prediv_offset: 0x2C,
+
+            dacpd_bit: 1 << 15,
+            dsmpd_bit: 1 << 16,
+
+            fbdiv_shift: 17,
+        },
+        PllDef {
+            pd_offset: 0x2C,
+            fbdiv_offset: 0x2C,
+            frac_offset: 0x30,
+            prediv_offset: 0x34,
+
+            dacpd_bit: 1 << 15,
+            dsmpd_bit: 1 << 16,
+
+            fbdiv_shift: 17,
+        },
+    ];
+
+    const FBDIV_MASK: u32 = 0xFFF;
+    const FRAC_MASK: u32 = 0xFFFFFF;
+    const POSTDIV1_MASK: u32 = 0x3;
+    const PREDIV_MASK: u32 = 0x3F;
+
+    const POSTDIV1_SHIFT: usize = 28;
+    const PREDIV_SHIFT: usize = 0;
+    const FRAC_SHIFT: usize = 0;
+
+    fn read_pll_regs(&self, index: usize) -> Result<PllRegs, Error> {
+        let pll = &Self::PLLS[index];
+
+        let val = self.syscon.read_register(pll.pd_offset)?;
+        let dacpd = val & pll.dacpd_bit != 0;
+        let dsmpd = val & pll.dsmpd_bit != 0;
+
+        let val = self.syscon.read_register(pll.fbdiv_offset)?;
+        let fbdiv = (val >> pll.fbdiv_shift) & Self::FBDIV_MASK;
+
+        let val = self.syscon.read_register(pll.frac_offset)?;
+        let frac = (val >> Self::FRAC_SHIFT) & Self::FRAC_MASK;
+        let postdiv1 = (val >> Self::POSTDIV1_SHIFT) & Self::POSTDIV1_MASK;
+
+        let val = self.syscon.read_register(pll.prediv_offset)?;
+        let prediv = (val >> Self::PREDIV_SHIFT) & Self::PREDIV_MASK;
+
+        Ok(PllRegs {
+            dacpd,
+            dsmpd,
+
+            fbdiv,
+            frac,
+            postdiv1,
+            prediv,
+        })
+    }
+}
+
 impl Device for Pll {
+    unsafe fn init(self: Arc<Self>, _cx: DeviceInitContext) -> Result<(), Error> {
+        // Dump PLL rates
+        for i in 0..3 {
+            let regs = self.read_pll_regs(i as usize)?;
+            let rate = self.clock_rate(Some(i))?;
+            log::info!("PLL{i} rate: {rate}");
+            log::info!("  {regs:?}");
+        }
+
+        Ok(())
+    }
+
     fn display_name(&self) -> &str {
         "jh7110-pll"
     }
@@ -404,8 +623,24 @@ impl ClockController for Pll {
         todo!("PLL rate configuration not yet implemented")
     }
 
-    fn clock_rate(&self, _clock: Option<u32>) -> Result<Hertz, Error> {
-        todo!("PLL rate query not yet implemented")
+    fn clock_rate(&self, clock: Option<u32>) -> Result<Hertz, Error> {
+        let index = clock.ok_or(Error::InvalidArgument)? as usize;
+        if index >= 3 {
+            return Err(Error::InvalidArgument);
+        }
+
+        let osc = self.clk_osc.rate()?;
+        let pll = self.read_pll_regs(index)?;
+
+        if pll.dacpd && pll.dsmpd {
+            // Integer mode
+            Ok((osc * pll.fbdiv) / (pll.prediv << pll.postdiv1))
+        } else if !pll.dacpd && !pll.dsmpd {
+            // Fraction mode
+            todo!()
+        } else {
+            todo!("Invalid PLL dacpd/dsmpd combination")
+        }
     }
 
     fn enable_clock(&self, _clock: Option<u32>) -> Result<(), Error> {
@@ -500,11 +735,52 @@ device_tree_driver! {
     }
 }
 
+device_tree_driver! {
+    compatible: ["starfive,jh7110-stgcrg"],
+    driver: {
+        fn probe(&self, node: &Arc<Node>, context: &mut ProbeContext) -> Option<Arc<dyn Device>> {
+            struct Stgcrg;
+
+            impl ClockDefinition for Stgcrg {
+                const NAME: &'static str = "jh7110-stgcrg";
+                const CLOCKS: &'static [Option<(&'static str, ClockDef)>] = STGCRG_CLOCKS;
+            }
+
+            let base = node.map_base(context, 0)?;
+            let parents = [
+                node.named_clock("osc").unwrap(),
+                node.named_clock("hifi4_core").unwrap(),
+                node.named_clock("stg_axiahb").unwrap(),
+                node.named_clock("usb_125m").unwrap(),
+                node.named_clock("cpu_bus").unwrap(),
+                node.named_clock("hifi4_axi").unwrap(),
+                node.named_clock("nocstg_bus").unwrap(),
+                node.named_clock("apb_bus").unwrap()
+            ];
+
+            let stgcrg = unsafe { Crg::<Stgcrg, _>::map(base, parents) }.ok()?;
+
+            node.make_clock_controller(stgcrg.clone());
+            node.make_reset_controller(stgcrg.clone());
+
+            Some(stgcrg)
+        }
+    }
+}
+
 device_tree_driver! {
     compatible: ["starfive,jh7110-pll"],
     driver: {
         fn probe(&self, node: &Arc<Node>, _context: &mut ProbeContext) -> Option<Arc<dyn Device>> {
-            let pll = Arc::new(Pll);
+            let clk_osc = node.clock(0)?; // 24MHz
+            // Make sure parent syscon is probed
+            let syscon_phandle = node.parent()?.prop_usize("phandle")? as u32;
+            let syscon = lookup_phandle(syscon_phandle, true)?.as_system_controller()?;
+
+            let pll = Arc::new(Pll {
+                syscon,
+                clk_osc,
+            });
 
             node.make_clock_controller(pll.clone());
 

kernel/src/device/mod.rs

@@ -8,6 +8,7 @@ pub mod clock;
 pub mod display;
 pub mod interrupt;
 pub mod mbox;
+pub mod pinctrl;
 pub mod power;
 pub mod serial;
 // pub mod timer;

kernel/src/device/pinctrl/jh7110_pinctrl.rs

@@ -0,0 +1,49 @@
+use abi::error::Error;
+use alloc::sync::Arc;
+use device_api::{
+    clock::ClockHandle,
+    device::{Device, DeviceInitContext},
+};
+use device_tree::driver::{device_tree_driver, Node, ProbeContext};
+use libk_mm::device::DeviceMemoryIoMut;
+use libk_util::sync::IrqSafeSpinlock;
+
+struct Jh7110SysPinctrl {
+    #[allow(unused)]
+    regs: IrqSafeSpinlock<DeviceMemoryIoMut<'static, [u32]>>,
+    clock: ClockHandle,
+}
+
+unsafe impl Send for Jh7110SysPinctrl {}
+unsafe impl Sync for Jh7110SysPinctrl {}
+
+impl Device for Jh7110SysPinctrl {
+    unsafe fn init(self: Arc<Self>, _cx: DeviceInitContext) -> Result<(), Error> {
+        self.clock.enable()?;
+
+        Ok(())
+    }
+
+    fn display_name(&self) -> &str {
+        "jh7110-sys-pinctrl"
+    }
+}
+
+device_tree_driver! {
+    compatible: ["starfive,jh7110-sys-pinctrl"],
+    driver: {
+        fn probe(&self, node: &Arc<Node>, context: &mut ProbeContext) -> Option<Arc<dyn Device>> {
+            let clock = node.clock(0).unwrap();
+            let base = node.map_base(context, 0)?;
+
+            let regs = unsafe { DeviceMemoryIoMut::map_slice(base, 0x2B8 / 4, Default::default()) }.ok()?;
+
+            let pinctrl = Arc::new(Jh7110SysPinctrl {
+                clock,
+                regs: IrqSafeSpinlock::new(regs)
+            });
+
+            Some(pinctrl)
+        }
+    }
+}

kernel/src/device/pinctrl/mod.rs

@@ -0,0 +1,4 @@
+//! GPIO/Pin controller, multiplexer drivers
+
+#[cfg(any(target_arch = "riscv64", rust_analyzer))]
+mod jh7110_pinctrl;

kernel/src/main.rs

@@ -74,12 +74,11 @@ extern crate ygg_driver_virtio_net;
 cfg_if::cfg_if! {
     if #[cfg(target_arch = "x86_64")] {
         extern crate ygg_driver_net_igbe;
+    } else if #[cfg(target_arch = "riscv64")] {
+        extern crate ygg_driver_net_stmmac;
     }
 }
 
-#[cfg(any(target_arch = "riscv64", rust_analyzer))]
-extern crate ygg_driver_net_stmmac;
-
 pub mod arch;
 pub mod device;
 pub mod fs;