rdrive/probe/fdt/
mod.rs

1use alloc::{
2    collections::{BTreeMap, btree_set::BTreeSet},
3    vec::Vec,
4};
5use core::ptr::NonNull;
6use spin::{Mutex, Once};
7
8pub use fdt_edit::InterruptRef;
9pub use fdt_edit::{ClockRef, Fdt, NodeType, Phandle, RegInfo, Status};
10
11use crate::{
12    Descriptor, DeviceId, PlatformDevice,
13    error::DriverError,
14    probe::OnProbeError,
15    register::{DriverRegister, ProbeKind},
16};
17
18use super::ProbeError;
19
20static SYSTEM: Once<System> = Once::new();
21
22pub fn init(fdt_addr: NonNull<u8>) -> Result<(), DriverError> {
23    let sys = System::new(fdt_addr)?;
24    SYSTEM.call_once(|| sys);
25    Ok(())
26}
27
28pub fn probe_register(
29    register: &DriverRegister,
30) -> Result<Vec<Result<(), OnProbeError>>, ProbeError> {
31    let sys = system();
32    sys.probe_register(register)
33}
34
35pub(crate) fn system() -> &'static System {
36    SYSTEM.get().expect("rdrive not init")
37}
38
39pub struct FdtInfo<'a> {
40    pub node: NodeType<'a>,
41    phandle_2_device_id: BTreeMap<Phandle, DeviceId>,
42}
43
44impl<'a> FdtInfo<'a> {
45    pub fn phandle_to_device_id(&self, phandle: Phandle) -> Option<DeviceId> {
46        self.phandle_2_device_id.get(&phandle).copied()
47    }
48
49    pub fn find_clk_by_name(&self, name: &str) -> Option<ClockRef> {
50        self.node
51            .clocks()
52            .into_iter()
53            .find(|clock| clock.name.as_deref() == Some(name))
54    }
55
56    pub fn interrupts(&self) -> Vec<InterruptRef> {
57        self.node.interrupts()
58    }
59}
60
61pub type FnOnProbe = fn(fdt: FdtInfo<'_>, plat_dev: PlatformDevice) -> Result<(), OnProbeError>;
62
63pub struct System {
64    fdt: Fdt,
65    phandle_2_device_id: BTreeMap<Phandle, DeviceId>,
66    probed_names: Mutex<BTreeSet<&'static str>>,
67}
68
69unsafe impl Send for System {}
70
71impl System {
72    pub fn phandle_to_device_id(&self, phandle: Phandle) -> Option<DeviceId> {
73        self.phandle_2_device_id.get(&phandle).copied()
74    }
75
76    pub fn new(fdt_addr: NonNull<u8>) -> Result<Self, DriverError> {
77        let fdt = unsafe { Fdt::from_ptr(fdt_addr.as_ptr()) }
78            .map_err(|error| DriverError::Fdt(format!("{error:?}")))?;
79        let mut phandle_2_device_id = BTreeMap::new();
80        for node in fdt.all_nodes() {
81            if let Some(phandle) = node.as_node().phandle() {
82                phandle_2_device_id.insert(phandle, DeviceId::new());
83            }
84        }
85        Ok(Self {
86            fdt,
87            phandle_2_device_id,
88            probed_names: Mutex::new(BTreeSet::new()),
89        })
90    }
91
92    fn new_device_id(&self, phandle: Option<Phandle>) -> DeviceId {
93        if let Some(phandle) = phandle {
94            self.phandle_2_device_id[&phandle]
95        } else {
96            DeviceId::new()
97        }
98    }
99
100    fn get_fdt_match_nodes<'a>(&'a self, register: &DriverRegister) -> Vec<ProbeFdtInfo<'a>> {
101        let mut out = Vec::new();
102        for node in self.fdt.all_nodes() {
103            if matches!(node.as_node().status(), Some(Status::Disabled)) {
104                continue;
105            }
106
107            let node_compatibles = node.as_node().compatibles().collect::<Vec<_>>();
108
109            for probe in register.probe_kinds {
110                let &ProbeKind::Fdt {
111                    compatibles,
112                    on_probe,
113                } = probe
114                else {
115                    continue;
116                };
117
118                for compatible in &node_compatibles {
119                    if compatibles.contains(compatible) {
120                        out.push(ProbeFdtInfo {
121                            name: register.name,
122                            node,
123                            on_probe,
124                        });
125                    }
126                }
127            }
128        }
129        out
130    }
131
132    fn probe_register(
133        &self,
134        register: &DriverRegister,
135    ) -> Result<Vec<Result<(), OnProbeError>>, ProbeError> {
136        let node_ls = self.get_fdt_match_nodes(register);
137        let mut out = Vec::new();
138        for node_info in node_ls {
139            if self.probed_names.lock().contains(node_info.name) {
140                continue;
141            }
142            let node = node_info.node;
143            let node_phandle = node.as_node().phandle();
144            let id = self.new_device_id(node_phandle);
145
146            let irq_parent = node
147                .interrupt_parent()
148                .filter(|p| Some(*p) != node_phandle)
149                .and_then(|p| self.phandle_2_device_id.get(&p).copied());
150
151            let phandle_map = self.phandle_2_device_id.clone();
152
153            debug!("Probe [{}]->[{}]", node.name(), node_info.name);
154
155            let descriptor = Descriptor {
156                name: node_info.name,
157                device_id: id,
158                irq_parent,
159            };
160
161            let res = (node_info.on_probe)(
162                FdtInfo {
163                    node,
164                    phandle_2_device_id: phandle_map,
165                },
166                PlatformDevice::new(descriptor),
167            );
168
169            if res.is_ok() {
170                self.probed_names.lock().insert(node_info.name);
171            }
172
173            out.push(res);
174        }
175
176        Ok(out)
177    }
178}
179
180struct ProbeFdtInfo<'a> {
181    name: &'static str,
182    node: NodeType<'a>,
183    on_probe: FnOnProbe,
184}
rdrive/probe/fdt/mod.rs

rdrive/probe/fdt/
mod.rs
