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

rdrive/probe/fdt/
mod.rs
