diff --git "a/docs/\350\277\233\345\272\246\350\267\237\350\270\252.md" "b/docs/\350\277\233\345\272\246\350\267\237\350\270\252.md" index e31ba5fcebae55..1d590ff0364038 100644 --- "a/docs/\350\277\233\345\272\246\350\267\237\350\270\252.md" +++ "b/docs/\350\277\233\345\272\246\350\267\237\350\270\252.md" @@ -21,6 +21,10 @@ - [ ] 完成代码编写并通过编译测试 4. [ ] 完成 i2c_msg 在 rust 下的实现 - [ ] 完成代码编写并通过编译测试 -5. [ ] 完成 bcm2835_i2c_probe 函数的实现(依赖前几项工作) +5. [ ] 完成 of_node 部分功能的实现 + - [ ] 完成of_node_full_name +6. [ ] 完成 interrupt mod 的部分功能实现 + - [ ] 完成 request_irq 的封装 +7. [ ] 完成 bcm2835_i2c_probe 函数的实现(依赖前几项工作) - [ ] 完成代码编写并通过编译测试 - [ ] 使用真实的 i2c 设备进行功能测试 diff --git a/drivers/i2c/busses/i2c_bcm2835_rust.rs b/drivers/i2c/busses/i2c_bcm2835_rust.rs index 1e481d316ff5af..b9cbc59e0740ba 100644 --- a/drivers/i2c/busses/i2c_bcm2835_rust.rs +++ b/drivers/i2c/busses/i2c_bcm2835_rust.rs @@ -1,9 +1,6 @@ // SPDX-License-Identifier: GPL-2.0 //! BCM2835 master mode driver - -use core::ops::Not; - use kernel::{ bindings, clk::Clk, @@ -13,7 +10,6 @@ use kernel::{ driver::DeviceRemoval, error::to_result, i2c::{self, I2cAdapter, I2cAdapterQuirks, I2cMsg, I2C_M_NOSTART, I2C_M_RD}, - io_mem::IoMem, irq, of::DeviceId, platform, @@ -74,9 +70,6 @@ pub const BCM2835_I2C_REDL_SHIFT: u32 = 0; pub const BCM2835_I2C_CDIV_MIN: u32 = 0x0002; pub const BCM2835_I2C_CDIV_MAX: u32 = 0xFFFE; -// Debug and Clk_tout_ms is static mut in C code. -// Take as const for now. - pub const DEBUG: i32 = 0; /// SMBUs-recommended 35ms @@ -100,7 +93,7 @@ struct Bcm2835I2cDev { reg_base: *mut u8, irq: i32, adapter: I2cAdapter, - completion: Completion, + completion: Arc, curr_msg: Option>, bus_clk: Clk, num_msgs: i32, @@ -112,19 +105,29 @@ struct Bcm2835I2cDev { debug_num_msgs: u32, } -fn to_clk_bcm2835_i2c(hw_ptr: &ClkHw) -> &mut ClkBcm2835I2c { - unsafe { &mut *(container_of!(hw_ptr, ClkBcm2835I2c, hw) as *mut ClkBcm2835I2c) } +impl Bcm2835I2cDev { + unsafe fn from_ptr<'a>(ptr: *mut Self) -> &'a mut Self { + unsafe { &mut *ptr.cast() } + } + + unsafe fn as_ptr(&self) -> *mut Self { + self as *const _ as *mut _ + } +} + +fn to_clk_bcm2835_i2c(hw_ptr: &ClkHw) -> &mut ClkBcm2835I2c<'_> { + unsafe { &mut *(container_of!(hw_ptr, ClkBcm2835I2c<'_>, hw) as *mut ClkBcm2835I2c<'_>) } } -struct ClkBcm2835I2c { +struct ClkBcm2835I2c<'c> { hw: ClkHw, - i2c_dev: &'static mut Bcm2835I2cDev, + i2c_dev: &'c Bcm2835I2cDev, } -impl ClkBcm2835I2c { +impl<'c> ClkBcm2835I2c<'c> { fn from_raw<'a>(ptr: *mut Self) -> &'a mut Self { - let prt = ptr.cast::(); - unsafe { &mut *prt } + let ptr = ptr.cast::(); + unsafe { &mut *ptr } } } @@ -186,12 +189,12 @@ fn clk_bcm2835_i2c_set_rate(hw: &ClkHw, rate: u64, parent_rate: u64) -> Result<( Ok(()) } -fn clk_bcm2835_i2c_round_rate(hw: &ClkHw, rate: u64, parent_rate: &mut u64) -> i32 { +fn clk_bcm2835_i2c_round_rate(hw: &ClkHw, rate: u64, parent_rate: &mut u64) -> i64 { let Ok(divider) = clk_bcm2835_i2c_calc_divider(rate, *parent_rate) else { return 0; }; - parent_rate.div_ceil(divider) as i32 + parent_rate.div_ceil(divider) as i64 } fn clk_bcm2835_i2c_recalc_rate(hw: &ClkHw, parent_rate: u64) -> u64 { @@ -220,48 +223,38 @@ impl ClkOps for ClkBcm2835I2cOps { } impl Bcm2835I2cDev { - pub(crate) fn bcm2835_i2c_writel(&mut self, reg: usize, val: u32) { + pub(crate) fn bcm2835_i2c_writel(&self, reg: usize, val: u32) { let i2c_reg = self.reg_base; let addr = i2c_reg.wrapping_add(reg); unsafe { bindings::writel(val as _, addr as _) } } - pub(crate) fn bcm2835_i2c_readl(&mut self, reg: usize) -> u32 { + pub(crate) fn bcm2835_i2c_readl(&self, reg: usize) -> u32 { let i2c_reg = self.reg_base; let addr = i2c_reg.wrapping_add(reg); unsafe { bindings::readl(addr as _) } } - pub(crate) fn bcm2835_i2c_register_div( - dev: &'static mut Device, - mclk: &Clk, - i2c_dev: &'static mut Bcm2835I2cDev, - ) -> Result<&'static mut Clk> { - let name = CString::try_from_fmt(fmt!("{}_div", dev.name()))?; + pub(crate) fn bcm2835_i2c_register_div(&mut self, mclk: &Clk) -> Result<&mut Clk> { + let name = CString::try_from_fmt(fmt!("{}_div", self.dev.name()))?; let mclk_name = mclk.name(); let parent_names = [mclk_name.as_char_ptr()]; - // Here: impl device.rs Device struct - // devm_alloc:: let clk_i2c = unsafe { - let raw_ptr = dev.kzalloc::()?; + let raw_ptr = self.dev.kzalloc::>()?; let clk_i2c = ClkBcm2835I2c::from_raw(raw_ptr); let init_data = ClkInitData::new() .name_config(&name, &parent_names) .set_ops::() .set_flags(0); clk_i2c.hw.set_init_data(&init_data); - clk_i2c.i2c_dev = i2c_dev; + clk_i2c.i2c_dev = self; - clk_i2c.hw.register_clkdev(c_str!("div"), dev.name())?; + clk_i2c.hw.register_clkdev(c_str!("div"), self.dev.name())?; clk_i2c }; - // Ensure these objects live long enough - // TODO: Try to achieve this in a more elegant way - // let _ = (name, parent_names, init_data); - - dev.clk_register(&mut clk_i2c.hw) + self.dev.clk_register(&mut clk_i2c.hw) } pub(crate) fn bcm2835_fill_txfifo(&mut self) { @@ -340,7 +333,7 @@ impl Bcm2835I2cDev { } } -fn bcm2835_i2c_isr(this_irq: i32, data: *mut core::ffi::c_void) -> irq::Return { +fn bcm2835_i2c_isr(this_irq: i32, data: &mut Bcm2835I2cDev) -> irq::Return { let i2c_dev = unsafe { &mut *(data as *mut Bcm2835I2cDev) }; let mut val: u32 = i2c_dev.bcm2835_i2c_readl(BCM2835_I2C_S); @@ -403,7 +396,7 @@ fn bcm2835_i2c_isr(this_irq: i32, data: *mut core::ffi::c_void) -> irq::Return { } unsafe extern "C" fn bcm2835_i2c_isr_cb(this_irq: i32, data: *mut core::ffi::c_void) -> u32 { - bcm2835_i2c_isr(this_irq, data) as u32 + bcm2835_i2c_isr(this_irq, unsafe { &mut *data.cast() }) as u32 } fn goto_complete(i2c_dev: &mut Bcm2835I2cDev) -> irq::Return { @@ -500,6 +493,10 @@ fn bcm2835_i2c_func(adap: I2cAdapter) -> u32 { struct Bcm2835I2cAlgo; struct Bcm2835I2cData {} + +unsafe impl Sync for Bcm2835I2cData {} +unsafe impl Send for Bcm2835I2cData {} + struct Bcm2835I2cDriver; module_platform_driver! { @@ -548,28 +545,41 @@ impl platform::Driver for Bcm2835I2cDriver { pdev.name() ); - let dev = unsafe { Device::new(pdev.raw_device()) }; + let dev = unsafe { Device::from_dev(pdev) }; let i2c_dev_ptr: *mut Bcm2835I2cDev = dev.kzalloc::()?; - let i2c_dev = unsafe { &mut (*i2c_dev_ptr) }; - i2c_dev.dev = dev; - i2c_dev.completion.reinit(); - i2c_dev.reg_base = pdev.ioremap_resource(0)?; - dev_info!(pdev, "I2c bus device reg_base: {:?}\n", i2c_dev.reg_base); + let i2c_dev = unsafe { Bcm2835I2cDev::from_ptr(i2c_dev_ptr) }; + i2c_dev.dev = dev.clone(); + i2c_dev.completion = Arc::pin_init(new_completion!())?; - let mclk = i2c_dev.dev.clk_get()?; + let reg_base = pdev.ioremap_resource(0)?; + i2c_dev.reg_base = reg_base; + dev_info!(pdev, "I2c bus device reg_base: {:?}\n", reg_base); - // TODO: initialise i2c bus clock - // let bus_clk = + let mclk = dev.clk_get()?; + + let bus_clk = i2c_dev.bcm2835_i2c_register_div(mclk)?; let mut bus_clk_rate = 0; - let ret = i2c_dev - .dev - .of_property_read_u32(c_str!("clock-frequency"), &mut bus_clk_rate); + if let Err(_) = dev.of_property_read_u32(c_str!("clock-frequency"), &mut bus_clk_rate) { + bus_clk_rate = bindings::I2C_MAX_STANDARD_MODE_FREQ; + }; dev_info!(pdev, "I2c bus device clock-frequency: {}\n", bus_clk_rate); + let ret = + unsafe { bindings::clk_set_rate_exclusive(bus_clk.as_ptr(), bus_clk_rate as u64) }; + if ret < 0 { + dev_err!( + pdev, + "Could not set clock frequency: {:?}\n", + to_result(ret) + ); + to_result(ret)?; + } + + i2c_dev.bus_clk.prepare_enable()?; + let irq = pdev.irq_resource(0)?; - i2c_dev.irq = irq; let ret = unsafe { bindings::request_threaded_irq( @@ -581,13 +591,53 @@ impl platform::Driver for Bcm2835I2cDriver { i2c_dev_ptr as *mut core::ffi::c_void, ) }; - if ret == 0 { + if ret < 0 { dev_err!(pdev, "Could not request IRQ: {}\n", irq); - // TODO: disable bus clock + to_result(ret)?; + } + i2c_dev.irq = irq; + + // TODO: setup i2c_adapter + let quirks = I2cAdapterQuirks::new().set_flags(i2c::I2C_AQ_NO_CLK_STRETCH as u64); + // let mut adap = i2c_dev.adapter; + unsafe { + i2c_dev.adapter.i2c_set_adapdata(i2c_dev.as_ptr()); + // TODO: set owner + // i2c_dev.adapter.set_owner((&bindings::__this_module) as *const _ as *mut _); + i2c_dev.adapter.set_class(bindings::I2C_CLASS_DEPRECATED); + let full_name = bindings::of_node_full_name((*pdev.raw_device()).of_node); + let adap_name = + CString::try_from_fmt(fmt!("bcm2835 ({})", CStr::from_char_ptr(full_name)))?; + i2c_dev.adapter.set_name(&adap_name); + } + // i2c_dev.adapter = adap; + + /* + * Disable the hardware clock stretching timeout. SMBUS + * specifies a limit for how long the device can stretch the + * clock, but core I2C doesn't. + */ + i2c_dev.bcm2835_i2c_writel(BCM2835_I2C_CLKT, 0); + i2c_dev.bcm2835_i2c_writel(BCM2835_I2C_C, 0); + + let ret = unsafe { bindings::i2c_add_adapter(i2c_dev.adapter.as_ptr()) }; + if ret < 0 { + dev_info!(pdev, "Could not add I2C adapter: {:?}\n", to_result(ret)); + unsafe { + bindings::free_irq(irq as u32, i2c_dev_ptr as *mut core::ffi::c_void); + } } + let _ = to_result(ret)?; let dev_data = kernel::new_device_data!((), (), Bcm2835I2cData {}, "BCM2835_I2C device data")?; + /* + * Disable the hardware clock stretching timeout. SMBUS + * specifies a limit for how long the device can stretch the + * clock, but core I2C doesn't. + */ + i2c_dev.bcm2835_i2c_writel(BCM2835_I2C_CLKT, 0); + i2c_dev.bcm2835_i2c_writel(BCM2835_I2C_C, 0); Ok(dev_data.into()) } diff --git a/rust/bindings/bindings_helper.h b/rust/bindings/bindings_helper.h index eff9348644896a..0b4b395476c667 100644 --- a/rust/bindings/bindings_helper.h +++ b/rust/bindings/bindings_helper.h @@ -33,6 +33,8 @@ #include #include #include +#include +#include /* `bindgen` gets confused at certain things. */ const size_t BINDINGS_ARCH_SLAB_MINALIGN = ARCH_SLAB_MINALIGN; diff --git a/rust/helpers.c b/rust/helpers.c index dfbf0d79e3f436..8c14deda181649 100644 --- a/rust/helpers.c +++ b/rust/helpers.c @@ -262,6 +262,12 @@ const struct of_device_id *rust_helper_of_match_device( } EXPORT_SYMBOL_GPL(rust_helper_of_match_device); +const char *rust_helper_of_node_full_name(const struct device_node *np) +{ + return rust_helper_of_node_full_name(np); +} +EXPORT_SYMBOL_GPL(rust_helper_of_node_full_name); + void *rust_helper_platform_get_drvdata(const struct platform_device *pdev) { return platform_get_drvdata(pdev); diff --git a/rust/kernel/clk.rs b/rust/kernel/clk.rs index f16eb5d62697ac..484651f2b9f387 100644 --- a/rust/kernel/clk.rs +++ b/rust/kernel/clk.rs @@ -72,6 +72,7 @@ impl Drop for Clk { fn drop(&mut self) { // SAFETY: The pointer is valid by the type invariant. unsafe { + bindings::clk_unprepare(self.0.get()); bindings::clk_rate_exclusive_put(self.0.get()); bindings::clk_put(self.0.get()); } diff --git a/rust/kernel/clk_provider.rs b/rust/kernel/clk_provider.rs index 6e5fed2e895f97..b23a887ab04e22 100644 --- a/rust/kernel/clk_provider.rs +++ b/rust/kernel/clk_provider.rs @@ -57,7 +57,7 @@ impl ClkHw { } // Register one clock lookup for a struct clk_hw - pub fn register_clkdev(&mut self, con_id: &'static CStr, dev_id: &'static CStr) -> Result { + pub fn register_clkdev(&mut self, con_id: &CStr, dev_id: &CStr) -> Result { let ret = unsafe { bindings::clk_hw_register_clkdev( self.0.get(), diff --git a/rust/kernel/completion.rs b/rust/kernel/completion.rs index 8774313e6887af..71e09327cd3b62 100644 --- a/rust/kernel/completion.rs +++ b/rust/kernel/completion.rs @@ -5,13 +5,7 @@ //! C header: [`include/linux/completion.h`] //! -use crate::{ - types::Opaque, - str::CStr, - sync::LockClassKey, - prelude::PinInit, - bindings, -}; +use crate::{bindings, prelude::PinInit, str::CStr, sync::LockClassKey, types::Opaque}; /// Linux completion wrapper /// @@ -42,8 +36,7 @@ impl Completion { /// Creates a new instance of [`Completion`]. #[inline] #[allow(clippy::new_ret_no_self)] - pub fn new(name: &'static CStr, key: LockClassKey) -> impl PinInit - { + pub fn new(name: &'static CStr, key: LockClassKey) -> impl PinInit { unsafe { kernel::init::pin_init_from_closure(move |slot| { let slot = Self::raw_get(slot); @@ -73,25 +66,34 @@ impl Completion { unsafe { Opaque::raw_get(core::ptr::addr_of!((*ptr).0)) } } - /// completion reinit + /// completion reinit pub fn reinit(&self) { - unsafe {(*(self.0.get())).done = 0;} + unsafe { + (*(self.0.get())).done = 0; + } } /// This waits to be signaled for completion of a specific task. It is NOT /// interruptible and there is no timeout. pub fn wait_for_completion(&self) { // SAFETY: call ffi and ptr is valid - unsafe{ - bindings::wait_for_completion(self.0.get()) - } + unsafe { bindings::wait_for_completion(self.0.get()) } } - /// complete + /// complete pub fn complete(&self) { // SAFETY: call ffi and ptr is valid + unsafe { bindings::complete(self.0.get()) } + } + + /// Initialize a dynamically allocated completion + pub fn init_completion(&self) { unsafe { - bindings::complete(self.0.get()) + (*(self.0.get())).done = 0; + let wait = &mut (*(self.0.get())).wait; + let name = concat!(stringify!(wait), "\0").as_ptr() as *const core::ffi::c_char; + let key = LockClassKey().as_ptr(); + bindings::__init_swait_queue_head(wait, name, key); } } @@ -104,10 +106,10 @@ impl Completion { return 0; } - if left_jiff/(bindings::HZ as usize) == 0 { + if left_jiff / (bindings::HZ as usize) == 0 { return 1; } else { - return left_jiff/(bindings::HZ as usize); + return left_jiff / (bindings::HZ as usize); } } @@ -117,7 +119,9 @@ impl Completion { fn wait_for_completion_timeout(&self, jiff: usize) -> usize { // SAFETY: call ffi and ptr is valid unsafe { - bindings::wait_for_completion_timeout(self.0.get(), jiff.try_into().unwrap()).try_into().unwrap() + bindings::wait_for_completion_timeout(self.0.get(), jiff.try_into().unwrap()) + .try_into() + .unwrap() } } } diff --git a/rust/kernel/device.rs b/rust/kernel/device.rs index c1b4ed84436f5c..6d1cf7e7ad025c 100644 --- a/rust/kernel/device.rs +++ b/rust/kernel/device.rs @@ -11,6 +11,7 @@ use crate::{ dev_err, error::{code::*, from_err_ptr, to_result, Result}, macros::pin_data, + of::DeviceNode, pin_init, pr_crit, str::CStr, sync::{ @@ -213,6 +214,13 @@ impl Device { Ok(Clk::from_raw(raw)) } + /// Get node from dev + pub fn of_node(&self) -> &mut DeviceNode { + // safety: IS_ENABLED(CONFIG_OF) || dev ptr is valid + let ptr = unsafe { (*(self.raw_device())).of_node }; + DeviceNode::from_raw(ptr) + } + /// Allocate a new clock, register it and return an opaque cookie pub fn clk_register(&self, clk_hw: &mut ClkHw) -> Result<&mut Clk> { // SAFETY: call ffi and ptr is valid @@ -288,7 +296,6 @@ impl Clone for Device { Device::from_dev(self) } } - /// Device data. /// /// When a device is removed (for whatever reason, for example, because the device was unplugged or diff --git a/rust/kernel/i2c.rs b/rust/kernel/i2c.rs index 53f2224b66667f..76cfbea485345f 100644 --- a/rust/kernel/i2c.rs +++ b/rust/kernel/i2c.rs @@ -1,8 +1,10 @@ use crate::{ + device::{Device, RawDevice}, error::{to_result, Result}, + str::CStr, types::{ForeignOwnable, Opaque}, }; -use alloc::vec::{self, Vec}; +use alloc::vec::Vec; use core::mem::MaybeUninit; use core::{ffi::c_void, marker::PhantomData}; use macros::vtable; @@ -56,6 +58,10 @@ impl I2cAdapterQuirks { Self(up) } + pub fn as_ptr(&self) -> *mut bindings::i2c_adapter_quirks { + &self.0 as *const _ as *mut _ + } + pub fn set_flags(mut self, flags: u64) -> Self { self.0.flags = flags; self @@ -125,9 +131,50 @@ impl I2cAdapter { to_result(ret) } + pub fn set_name(&mut self, name: &CStr) { + let len = name.len().min(self.0.name.len() - 1); + let s = name.as_bytes(); + for b in &s[0..len] { + self.0.name[0] = *b as i8; + } + self.0.name[len] = 0; + } + + pub unsafe fn set_owner(&mut self, owner: *mut bindings::module) { + self.0.owner = owner + } + + pub fn set_class(&mut self, class: u32) { + self.0.class = class + } + + pub unsafe fn set_algorithm(&mut self, algorithm: *const bindings::i2c_algorithm) { + self.0.algo = algorithm + } + + pub unsafe fn setup_device(&mut self, device: &Device) { + let dev_ptr = device.raw_device(); + self.0.dev.parent = dev_ptr; + unsafe { + self.0.dev.of_node = (*dev_ptr).of_node; + } + } + + pub unsafe fn set_quirks(&mut self, quirks: &I2cAdapterQuirks) { + self.0.quirks = &quirks.0 as *const _; + } + pub fn timeout(&self) -> usize { unsafe { self.0.timeout as usize } } + + //pub fn set_up(self) +} + +impl Drop for I2cAdapter { + fn drop(&mut self) { + unsafe { bindings::i2c_del_adapter(self.as_ptr()) } + } } /// Represents i2c_smbus_data /// diff --git a/rust/kernel/interrupt.rs b/rust/kernel/interrupt.rs new file mode 100644 index 00000000000000..3a7f0271af8113 --- /dev/null +++ b/rust/kernel/interrupt.rs @@ -0,0 +1,130 @@ +// SPDX-License-Identifier: GPL-2.0 + +//! Generic devices that are part of the kernel's driver model. +//! +//! C header: [`include/linux/device.h`](../../../../include/linux/.h) + +use crate::{ + device::{Device, RawDevice}, + error::{to_result, Result}, + irq, + str::CStr, +}; +use core::{marker::PhantomData, task::Context}; + +pub const IRQF_TRIGGER_NONE: u32 = bindings::IRQF_TRIGGER_NONE; +pub const IRQF_TRIGGER_RISING: u32 = bindings::IRQF_TRIGGER_RISING; +pub const IRQF_TRIGGER_FALLING: u32 = bindings::IRQF_TRIGGER_FALLING; +pub const IRQF_TRIGGER_HIGH: u32 = bindings::IRQF_TRIGGER_HIGH; +pub const IRQF_TRIGGER_LOW: u32 = bindings::IRQF_TRIGGER_LOW; +pub const IRQF_TRIGGER_MASK: u32 = bindings::IRQF_TRIGGER_MASK; +pub const IRQF_TRIGGER_PROBE: u32 = bindings::IRQF_TRIGGER_PROBE; +pub const IRQF_SHARED: u32 = bindings::IRQF_SHARED; +pub const IRQF_PROBE_SHARED: u32 = bindings::IRQF_PROBE_SHARED; +pub const __IRQF_TIMER: u32 = bindings::__IRQF_TIMER; +pub const IRQF_PERCPU: u32 = bindings::IRQF_PERCPU; +pub const IRQF_NOBALANCING: u32 = bindings::IRQF_NOBALANCING; +pub const IRQF_IRQPOLL: u32 = bindings::IRQF_IRQPOLL; +pub const IRQF_ONESHOT: u32 = bindings::IRQF_ONESHOT; +pub const IRQF_NO_SUSPEND: u32 = bindings::IRQF_NO_SUSPEND; +pub const IRQF_FORCE_RESUME: u32 = bindings::IRQF_FORCE_RESUME; +pub const IRQF_NO_THREAD: u32 = bindings::IRQF_NO_THREAD; +pub const IRQF_EARLY_RESUME: u32 = bindings::IRQF_EARLY_RESUME; +pub const IRQF_COND_SUSPEND: u32 = bindings::IRQF_COND_SUSPEND; +pub const IRQF_NO_AUTOEN: u32 = bindings::IRQF_NO_AUTOEN; +pub const IRQF_NO_DEBUG: u32 = bindings::IRQF_NO_DEBUG; +pub const IRQF_TIMER: u32 = bindings::IRQF_TIMER; +pub const IRQ_NOTCONNECTED: u32 = bindings::IRQ_NOTCONNECTED; + +pub trait IrqHandler { + /// User data that will be accessible to all operations + type Context; + + fn handler(irq: i32, ctx: &mut Self::Context) -> irq::Return; +} + +pub(crate) struct Adapter(PhantomData, PhantomData) +where + H: IrqHandler; + +impl Adapter +where + H: IrqHandler, +{ + unsafe extern "C" fn handler_callback( + arg1: i32, + arg2: *mut core::ffi::c_void, + ) -> bindings::irqreturn_t { + let dev = unsafe { &mut *(arg2 as *const _ as *mut _) }; + H::handler(arg1, dev) as bindings::irqreturn_t + } + + const VTABLE: bindings::irq_handler_t = Some(Self::handler_callback); + + const fn build() -> &'static bindings::irq_handler_t { + &Self::VTABLE + } +} + +pub fn request_irq(irq: u32, handler: H, flags: u64, name: &'static CStr, dev: &T) -> Result +where + H: IrqHandler, +{ + let ret = unsafe { + bindings::request_threaded_irq( + irq, + *Adapter::::build(), + None, + flags, + name.as_char_ptr(), + dev as *const _ as *mut core::ffi::c_void, + ) + }; + to_result(ret) +} + +pub fn request_percpu_irq( + irq: u32, + handler: H, + dev_name: &'static CStr, + percpu_dev_id: &T, +) -> Result +where + H: IrqHandler, +{ + let ret = unsafe { + bindings::__request_percpu_irq( + irq, + *Adapter::::build(), + 0, + dev_name.as_char_ptr(), + percpu_dev_id as *const _ as *mut core::ffi::c_void, + ) + }; + to_result(ret) +} + +pub fn devm_request_irq( + dev: Device, + irq: u32, + handler: H, + flags: u64, + dev_name: &'static CStr, + dev_id: &T, +) -> Result +where + H: IrqHandler, +{ + let ret = unsafe { + bindings::devm_request_threaded_irq( + dev.raw_device(), + irq, + *Adapter::::build(), + None, + flags, + dev_name.as_char_ptr(), + dev_id as *const _ as *mut core::ffi::c_void, + ) + }; + to_result(ret) +} diff --git a/rust/kernel/io_pgtable.rs b/rust/kernel/io_pgtable.rs new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/rust/kernel/lib.rs b/rust/kernel/lib.rs index 45a1c9a684c472..514f89646f9fad 100644 --- a/rust/kernel/lib.rs +++ b/rust/kernel/lib.rs @@ -78,6 +78,7 @@ pub mod task; pub mod timekeeping; pub mod types; pub mod user_ptr; +pub mod interrupt; use core::marker::PhantomData; diff --git a/rust/kernel/of.rs b/rust/kernel/of.rs index 3882350425c5c1..c77f3da713ec20 100644 --- a/rust/kernel/of.rs +++ b/rust/kernel/of.rs @@ -88,3 +88,30 @@ impl DeviceId { id } } + +pub struct Property(bindings::property); + +impl Property { + #[inline] + pub fn as_ptr(&self) -> *const bindings::property { + &self.0 + } +} + +pub struct DeviceNode(bindings::device_node); + +impl DeviceNode { + pub fn from_raw<'a>(ptr: *mut bindings::device_node) -> &'a mut Self { + let ptr = ptr.cast::(); + unsafe { &mut *ptr } + } + + #[inline] + pub fn as_ptr(&self) -> *const bindings::device_node { + &self.0 + } + + pub fn flags(&self) -> u64 { + self.0._flags + } +}