diff --git a/drivers/iio/common/aop_sensors/aop_als.rs b/drivers/iio/common/aop_sensors/aop_als.rs index 91170c2c1cd065..3047fd7ba5d8bd 100644 --- a/drivers/iio/common/aop_sensors/aop_als.rs +++ b/drivers/iio/common/aop_sensors/aop_als.rs @@ -8,7 +8,7 @@ use kernel::{ bindings, c_str, device::Core, firmware::Firmware, - iio::common::aop_sensors::{AopSensorData, IIORegistration, MessageProcessor, MICRO}, + iio::common::aop_sensors::{AopSensorData, IIORegistration, MessageProcessor, MICRO, MAX_CHANS}, module_platform_driver, of, platform, prelude::*, soc::apple::aop::{EPICService, FakehidRegistration, AOP}, @@ -47,6 +47,35 @@ const LUX_OFFSET_VD6286: usize = 0x28; */ const LUX_OFFSET_CT817: usize = 0x1c; +/// Slots in a CT720 decode. Slot 0 is the lux channel the driver already published. +const CH_LUX: usize = 0; +const CH_RED: usize = 1; +const CH_GREEN: usize = 2; +const CH_BLUE: usize = 3; +const CH_CLEAR: usize = 4; +const CH_CCT: usize = 5; +const CT720_CHANS: usize = 6; + +// FireFish2 / CT720 report, 55 bytes, lux float at +0x1d. The four counts are +// red, green, blue, clear. Clear is the wideband count (largest in room light; +// the calibration gains are 8, 8, 8, 4). Under the ~5460 K light captured on +// this J313 the first three ranked green, red, blue. Byte +0x2a is non-zero +// when the color temperature at +0x2b is valid. The integration time is stored +// twice at +0x2f (the AOP logs 197380 µs when it arms this sensor); disagreeing +// copies mean this is not that layout, and color is left unpublished. +const CT720_LEN: usize = 55; +const CT720_COUNT_OFF: [usize; 4] = [0x0d, 0x11, 0x15, 0x19]; +const CT720_CCT_FLAG: usize = 0x2a; +const CT720_CCT_OFF: usize = 0x2b; +const CT720_INTEG_OFF: usize = 0x2f; +const CT720_INTEG_MAX: u32 = 10_000_000; +/// Clear counts at or below 11 on a covered J313 still had the firmware CCT +/// byte set, and the published temperatures were 788 K, 1288 K, 2904 K and +/// 6781 K. Publish CCT only when clear is at least this. Not hardware-tested. +const CT720_CCT_MIN_CLEAR: u32 = 12; + +const _: () = assert!(CT720_CHANS <= MAX_CHANS); + fn get_lux_offset(aop: &dyn AOP, dev: &platform::Device, svc: &EPICService) -> Result { let name = get_aop_property(aop, svc, 0xf, 16)?.1; match name.as_slice() { @@ -153,6 +182,137 @@ fn f32_to_micro(f: u32) -> Option { }) } +fn u32_at(message: &[u8], off: usize) -> Option { + let b = message.get(off..off + 4)?; + Some(u32::from_le_bytes([b[0], b[1], b[2], b[3]])) +} + +/// Color slots of a CT720 report. Lux is filled by [`MsgProc::process`]. +fn ct720_color(message: &[u8], out: &mut [u64]) -> u32 { + if message.len() < CT720_LEN { + return 0; + } + let Some(integ) = u32_at(message, CT720_INTEG_OFF) else { + return 0; + }; + let Some(again) = u32_at(message, CT720_INTEG_OFF + 4) else { + return 0; + }; + if integ == 0 || integ != again || integ > CT720_INTEG_MAX { + return 0; + } + let mut mask = 0u32; + let mut clear = 0u32; + let mut slot = CH_RED; + for off in CT720_COUNT_OFF { + if let Some(count) = u32_at(message, off) { + if slot == CH_CLEAR { + clear = count; + } + if let Some(dest) = out.get_mut(slot) { + *dest = u64::from(count).saturating_mul(MICRO); + mask |= 1u32 << slot; + } + } + slot += 1; + } + if message.get(CT720_CCT_FLAG).copied() == Some(0) || clear < CT720_CCT_MIN_CLEAR { + return mask; + } + if let Some(bits) = u32_at(message, CT720_CCT_OFF) { + if bits & 0x8000_0000 == 0 { + if let Some(cct) = f32_to_micro(bits) { + if cct > 0 { + if let Some(dest) = out.get_mut(CH_CCT) { + *dest = cct; + mask |= 1u32 << CH_CCT; + } + } + } + } + } + mask +} + +fn chan_spec( + ty: u32, + channel2: i32, + modified: u32, + info_mask: usize, + index: usize, +) -> bindings::iio_chan_spec { + let mut spec = bindings::iio_chan_spec { + type_: ty, + channel2, + address: index, + // Polled sysfs channels. A negative scan index keeps them out of a buffer. + scan_index: -1, + __bindgen_anon_1: bindings::iio_chan_spec__bindgen_ty_1 { + scan_type: bindings::iio_scan_type { + __bindgen_anon_1: bindings::iio_scan_type__bindgen_ty_1 { + format: b'u' as _, + }, + realbits: 32, + storagebits: 32, + ..Default::default() + }, + }, + info_mask_separate: info_mask, + ..Default::default() + }; + spec.set_modified(modified); + spec +} + +fn ct720_channels() -> [bindings::iio_chan_spec; CT720_CHANS] { + let raw: usize = 1 << bindings::BINDINGS_IIO_CHAN_INFO_RAW; + let processed: usize = 1 << bindings::BINDINGS_IIO_CHAN_INFO_PROCESSED; + [ + chan_spec( + bindings::iio_chan_type_IIO_LIGHT, + 0, + 0, + processed, + CH_LUX, + ), + chan_spec( + bindings::iio_chan_type_IIO_INTENSITY, + bindings::iio_modifier_IIO_MOD_LIGHT_RED as i32, + 1, + raw, + CH_RED, + ), + chan_spec( + bindings::iio_chan_type_IIO_INTENSITY, + bindings::iio_modifier_IIO_MOD_LIGHT_GREEN as i32, + 1, + raw, + CH_GREEN, + ), + chan_spec( + bindings::iio_chan_type_IIO_INTENSITY, + bindings::iio_modifier_IIO_MOD_LIGHT_BLUE as i32, + 1, + raw, + CH_BLUE, + ), + chan_spec( + bindings::iio_chan_type_IIO_INTENSITY, + bindings::iio_modifier_IIO_MOD_LIGHT_CLEAR as i32, + 1, + raw, + CH_CLEAR, + ), + chan_spec( + bindings::iio_chan_type_IIO_COLORTEMP, + 0, + 0, + processed, + CH_CCT, + ), + ] +} + struct MsgProc(usize); impl MessageProcessor for MsgProc { @@ -160,6 +320,20 @@ impl MessageProcessor for MsgProc { let raw = message.get(self.0..self.0 + 4)?; f32_to_micro(u32::from_le_bytes([raw[0], raw[1], raw[2], raw[3]])) } + + fn decode(&self, message: &[u8], out: &mut [u64]) -> u32 { + let mut mask = 0u32; + if let Some(lux) = self.process(message) { + if let Some(dest) = out.get_mut(CH_LUX) { + *dest = lux; + mask |= 1u32 << CH_LUX; + } + } + if self.0 == LUX_OFFSET_CT720 { + mask |= ct720_color(message, out); + } + mask + } } struct IIOAopAlsDriver { @@ -203,8 +377,23 @@ impl platform::Driver for IIOAopAlsDriver { let listener = FakehidRegistration::new(adata.clone(), service, data.clone())?; enable_als(adata.as_ref(), pdev, &service, offset)?; let info_mask = 1 << bindings::BINDINGS_IIO_CHAN_INFO_PROCESSED; - let iio = - IIORegistration::::new(data, c"aop-sensors-als", ty, info_mask, &THIS_MODULE)?; + let iio = if offset == LUX_OFFSET_CT720 { + let channels = ct720_channels(); + IIORegistration::::with_channels( + data, + c"aop-sensors-als", + &channels, + &THIS_MODULE, + )? + } else { + IIORegistration::::new( + data, + c"aop-sensors-als", + ty, + info_mask, + &THIS_MODULE, + )? + }; Ok(IIOAopAlsDriver { listener, _iio: iio, diff --git a/rust/kernel/iio/common/aop_sensors.rs b/rust/kernel/iio/common/aop_sensors.rs index 7c80cef56b92ff..ff483c4b09be8b 100644 --- a/rust/kernel/iio/common/aop_sensors.rs +++ b/rust/kernel/iio/common/aop_sensors.rs @@ -15,10 +15,12 @@ use kernel::{ sync::{ aref::ARef, atomic::{ - Acquire, Atomic, - Relaxed, - Release, // + Relaxed, // + }, + barrier::{ + smp_rmb, + smp_wmb, // }, Arc, // }, @@ -33,51 +35,121 @@ pub const MICRO: u64 = 1_000_000; /// the `i32` values IIO reports. Larger readings saturate. pub const MAX_MICRO: u64 = i32::MAX as u64 * MICRO + (MICRO - 1); +/// How many readings one sensor can publish from a single report. +pub const MAX_CHANS: usize = 6; + /// Decodes one fake-HID report of a sensor into the value the IIO channel exposes. /// /// It is called from the AOP driver's receive worker while the IIO device may be read from /// another thread, hence `Send + Sync`. pub trait MessageProcessor: Send + Sync { - /// Returns the reading carried by `message` in micro-units of the channel (millionths of a + /// Returns the reading carried by `message` in micro-units of channel 0 (millionths of a /// lux, of a degree, ...), or `None` when the message carries no reading. fn process(&self, message: &[u8]) -> Option; + + /// Fills `out` with this report's channel values, in micro-units, and returns a bit mask of + /// the slots it wrote. Bit 0 is the channel [`IIORegistration::new`] registers. A zero mask + /// leaves the previous readings in place. + fn decode(&self, message: &[u8], out: &mut [u64]) -> u32 { + match self.process(message) { + Some(value) if !out.is_empty() => { + out[0] = value; + 1 + } + _ => 0, + } + } } -/// The state shared between a sensor's report listener and its IIO device: the latest reading. +/// The state shared between a sensor's report listener and its IIO device. pub struct AopSensorData { dev: ARef, + /// Type of channel 0. Extra channels are selected by `iio_chan_spec.address`. ty: u32, - /// The latest reading in micro-units, at most [`MAX_MICRO`]. - value: Atomic, - /// Whether a reading has arrived at all; until then reads return `ENODATA`. - valid: Atomic, + /// Even while the slots are stable, odd while a report is being published. + seq: Atomic, + /// Latest readings in micro-units, at most [`MAX_MICRO`]. + values: [Atomic; MAX_CHANS], + /// Bit per channel: set once that slot holds a reading. + valid: Atomic, msg_proc: T, } impl AopSensorData { - /// Creates the state for one channel of type `ty` whose reports `msg_proc` decodes. + /// Creates the state for a sensor whose channel 0 has type `ty` and whose reports `msg_proc` + /// decodes. pub fn new(dev: ARef, ty: u32, msg_proc: T) -> Result>> { Ok(Arc::new( AopSensorData { dev, ty, - value: Atomic::new(0), - valid: Atomic::new(false), + seq: Atomic::new(0), + values: [ + Atomic::new(0), + Atomic::new(0), + Atomic::new(0), + Atomic::new(0), + Atomic::new(0), + Atomic::new(0), + ], + valid: Atomic::new(0), msg_proc, }, GFP_KERNEL, )?) } + + /// One stable reading, or an errno when none has arrived or a publish is in progress. + fn load(&self, idx: usize) -> Result { + // The writer holds the sequence odd only across a few stores. A reader that keeps + // spinning can deadlock if it preempted that writer on this CPU, so give up. + let mut spins = 0; + while spins < 8 { + let seq = self.seq.load(Relaxed); + smp_rmb(); + if seq & 1 != 0 { + spins += 1; + continue; + } + let valid = self.valid.load(Relaxed); + let value = self.values[idx].load(Relaxed); + smp_rmb(); + if self.seq.load(Relaxed) != seq { + spins += 1; + continue; + } + if valid & (1u64 << idx) == 0 { + return Err(ENODATA.to_errno()); + } + return Ok(value); + } + Err(EAGAIN.to_errno()) + } } impl FakehidListener for AopSensorData { fn process_fakehid_report(&self, data: &[u8]) -> Result<()> { + let mut raw = [0u64; MAX_CHANS]; + let mask = self.msg_proc.decode(data, &mut raw); // A report without a reading leaves the last reading in place. - if let Some(value) = self.msg_proc.process(data) { - self.value.store(value.min(MAX_MICRO), Relaxed); - // Publishes the reading above to readers that see `valid`. - self.valid.store(true, Release); + if mask == 0 { + return Ok(()); } + let seq = self.seq.load(Relaxed); + self.seq.store(seq.wrapping_add(1), Relaxed); + smp_wmb(); + for i in 0..MAX_CHANS { + if (mask & (1u32 << i)) != 0 { + self.values[i].store(raw[i].min(MAX_MICRO), Relaxed); + } + } + // Replace, do not OR. A CT720 report that fails the layout check + // contributes only the lux bit, and the previous color sample must + // read back as ENODATA rather than stay published. A zero mask never + // reaches here, so a report with no reading keeps the last sample. + self.valid.store(u64::from(mask), Relaxed); + smp_wmb(); + self.seq.store(seq.wrapping_add(2), Relaxed); Ok(()) } } @@ -91,20 +163,22 @@ unsafe extern "C" fn aop_read_raw( val2: *mut i32, mask: isize, ) -> i32 { - // SAFETY: The IIO core calls this with the device and channel that `IIORegistration::new()` + // SAFETY: The IIO core calls this with the device and channel that `IIORegistration` // registered; `priv_` is the `Arc` it stored there. let data = unsafe { Arc::>::borrow((*dev).priv_.cast()) }; // SAFETY: `chan` is one of the registered channel specs. - let ty = unsafe { (*chan).type_ }; - if data.ty != ty { + let (idx, ty) = unsafe { ((*chan).address, (*chan).type_) }; + if idx >= MAX_CHANS { return EINVAL.to_errno(); } - // A sensor that has not reported yet, for instance because it lacks its - // calibration, has no reading rather than a reading of zero. - if !data.valid.load(Acquire) { - return ENODATA.to_errno(); + // Channel 0 is the original sensor type. Later channels carry their own type. + if idx == 0 && data.ty != ty { + return EINVAL.to_errno(); } - let micro = data.value.load(Relaxed); + let micro = match data.load(idx) { + Ok(micro) => micro, + Err(err) => return err, + }; // Both parts fit an `i32` because stored readings never exceed `MAX_MICRO`. let (int, frac) = ((micro / MICRO) as i32, (micro % MICRO) as i32); // SAFETY: `val` and `val2` point to the caller's result variables. @@ -122,7 +196,8 @@ unsafe extern "C" fn aop_read_raw( } struct IIOSpec { - spec: [bindings::iio_chan_spec; 1], + spec: [bindings::iio_chan_spec; MAX_CHANS], + n: usize, vtable: bindings::iio_info, _p: PhantomPinned, } @@ -135,6 +210,24 @@ pub struct IIORegistration { _p: PhantomData>, } +fn one_channel(ty: u32, info_mask: usize) -> bindings::iio_chan_spec { + bindings::iio_chan_spec { + type_: ty, + __bindgen_anon_1: bindings::iio_chan_spec__bindgen_ty_1 { + scan_type: bindings::iio_scan_type { + __bindgen_anon_1: bindings::iio_scan_type__bindgen_ty_1 { + format: b'u' as _, + }, + realbits: 32, + storagebits: 32, + ..Default::default() + }, + }, + info_mask_separate: info_mask, + ..Default::default() + } +} + impl IIORegistration { /// TODO: add documentation pub fn new( @@ -144,23 +237,26 @@ impl IIORegistration { info_mask: usize, module: &ThisModule, ) -> Result { + Self::with_channels(data, name, &[one_channel(ty, info_mask)], module) + } + + /// Registers `channels` on one IIO device. Each spec's `address` is the slot `decode` fills. + /// `channels` must be non-empty and no longer than [`MAX_CHANS`]. + pub fn with_channels( + data: Arc>, + name: &'static CStr, + channels: &[bindings::iio_chan_spec], + module: &ThisModule, + ) -> Result { + if channels.is_empty() || channels.len() > MAX_CHANS { + return Err(EINVAL); + } + let mut spec = [one_channel(0, 0); MAX_CHANS]; + spec[..channels.len()].copy_from_slice(channels); let spec = KBox::pin( IIOSpec { - spec: [bindings::iio_chan_spec { - type_: ty, - __bindgen_anon_1: bindings::iio_chan_spec__bindgen_ty_1 { - scan_type: bindings::iio_scan_type { - __bindgen_anon_1: bindings::iio_scan_type__bindgen_ty_1 { - format: b'u' as _, - }, - realbits: 32, - storagebits: 32, - ..Default::default() - }, - }, - info_mask_separate: info_mask, - ..Default::default() - }], + spec, + n: channels.len(), vtable: bindings::iio_info { read_raw: Some(aop_read_raw::), ..Default::default() @@ -184,7 +280,7 @@ impl IIORegistration { (*this.dev).name = name.as_ptr() as _; // spec is now pinned (*this.dev).channels = this.spec.spec.as_ptr(); - (*this.dev).num_channels = this.spec.spec.len() as i32; + (*this.dev).num_channels = this.spec.n as i32; (*this.dev).info = &this.spec.vtable; } let ret = unsafe { bindings::__iio_device_register(this.dev, module.as_ptr()) };