From a7ca7357078496c44ba6c262c51e5518e544d20b Mon Sep 17 00:00:00 2001 From: James Calligeros Date: Tue, 14 Feb 2023 20:30:57 +1000 Subject: [PATCH] implement skeleton temp model wip, probably horrible Signed-off-by: James Calligeros --- j314.conf | 14 +++++- src/helpers.rs | 48 +++++++++++++++++- src/main.rs | 28 +++++++---- src/types.rs | 134 ++++++++++++++++++++----------------------------- 4 files changed, 131 insertions(+), 93 deletions(-) diff --git a/j314.conf b/j314.conf index adbf06e..ec9465f 100644 --- a/j314.conf +++ b/j314.conf @@ -7,6 +7,8 @@ tau_magnet = 5 tr_coil = 6 ramp_factor = 7 temp_limit = 100 +vs_chan = 1 +is_chan = 2 [Right Tweeter] r_shunt = 1 @@ -16,7 +18,9 @@ tau_coil = 4 tau_magnet = 5 tr_coil = 6 ramp_factor = 7 -temp_limit = 100 +temp_limit = 100 +vs_chan = 3 +is_chan = 4 [Left Woofer 1] r_shunt = 1 @@ -27,6 +31,8 @@ tau_magnet = 5 tr_coil = 6 ramp_factor = 7 temp_limit = 100 +vs_chan = 5 +is_chan = 6 [Right Woofer 1] r_shunt = 1 @@ -37,6 +43,8 @@ tau_magnet = 5 tr_coil = 6 ramp_factor = 7 temp_limit = 100 +vs_chan = 7 +is_chan = 8 [Left Woofer 2] r_shunt = 1 @@ -47,6 +55,8 @@ tau_magnet = 5 tr_coil = 6 ramp_factor = 7 temp_limit = 100 +vs_chan = 9 +is_chan = 10 [Right Woofer 2] r_shunt = 1 @@ -57,3 +67,5 @@ tau_magnet = 5 tr_coil = 6 ramp_factor = 7 temp_limit = 100 +vs_chan = 11 +is_chan = 12 diff --git a/src/helpers.rs b/src/helpers.rs index f9eb071..ae00528 100644 --- a/src/helpers.rs +++ b/src/helpers.rs @@ -3,6 +3,7 @@ use configparser::ini::Ini; use alsa::mixer::MilliBel; +use alsa; /** Failsafe: Limit speaker volume massively and bail. @@ -27,16 +28,59 @@ pub fn open_card(card: &str) -> alsa::ctl::Ctl { return ctldev; } +pub fn open_pcm(dev: &str, chans: &u32) -> alsa::pcm::PCM { + let pcm = alsa::pcm::PCM::new(dev, alsa::Direction::Capture, false) + .unwrap(); + { + let params = alsa::pcm::HwParams::any(&pcm).unwrap(); + + params.set_channels(*chans).unwrap(); + params.set_rate(44100, alsa::ValueOr::Nearest).unwrap(); + params.set_format(alsa::pcm::Format::s16()).unwrap(); + params.set_access(alsa::pcm::Access::RWNonInterleaved).unwrap(); + pcm.hw_params(¶ms).unwrap(); + } + + return pcm; +} + +/** + Wrapper around configparser::ini::Ini.getint() + to safely unwrap the Result, E> returned by + it. +*/ +pub fn parse_int(config: &Ini, section: &str, key: &str) -> i64 { + let _result: Option = match config.getint(section, key) { + Ok(result) => match result{ + Some(inner) => { + let integer: i64 = inner; + return integer; + }, + None => { + println!("{}: Failed to parse {}", section, key); + fail(); + std::process::exit(1); + }, + }, + Err(e) => { + println!("{}: Invalid value for {}. Error: {}", section, key, e); + fail(); + std::process::exit(1); + }, + }; + +} + /** Wrapper around configparser::ini::Ini.getfloat() to safely unwrap the Result, E> returned by it. */ -pub fn parse_float(config: &Ini, section: &str, key: &str) -> f64 { +pub fn parse_float(config: &Ini, section: &str, key: &str) -> f32 { let _result: Option = match config.getfloat(section, key) { Ok(result) => match result{ Some(inner) => { - let float: f64 = inner; + let float: f32 = inner as f32; return float; }, None => { diff --git a/src/main.rs b/src/main.rs index 56e212c..f63d3de 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,15 +1,11 @@ // SPDX-License-Identifier: MIT // (C) 2022 The Asahi Linux Contributors - - /** Handles speaker safety on Apple Silicon machines. This code is designed to fail safe. The speaker should not be enabled until this daemon has successfully initialised. If at any time we run into an unrecoverable error (we shouldn't), we gracefully bail and use an IOCTL to shut off the speakers. */ - - use std::io; use std::fs::read_to_string; @@ -21,14 +17,16 @@ mod helpers; use crate::types::SafetyMonitor; static ASAHI_DEVICE: &str = "hw:0"; +static VISENSE_PCM: &str = "hw:0,2"; // Will eventually be /etc/speakersafetyd/ or similar static CONFIG_DIR: &str = "./"; -static SUPPORTED: [&str; 2] = [ +static SUPPORTED: [&str; 1] = [ "j314", - "j316", ]; +const BUF_SZ: usize = 128 * 6 * 2; + fn get_machine() -> String { let _compat: io::Result = match read_to_string("/proc/device-tree/compatible") { Ok(compat) => { @@ -71,10 +69,18 @@ fn main() { speakers.push(new_speaker); } - // Temporary to check that everything works. Threaded eventually if necessary. - for mut i in speakers { - i.run(&card); + let num_chans: u32 = speakers.len().try_into().unwrap(); + + // Set up PCM to buffer in V/ISENSE + let cap: alsa::pcm::PCM = helpers::open_pcm(&VISENSE_PCM, &num_chans); + let mut buf = [0i16; BUF_SZ]; // 128 samples from V and I for 6 channels + let io = cap.io_i16().unwrap(); + + loop { + // Block while we're reading into the buffer + assert_eq!(io.readi(&mut buf).unwrap(), buf.len()); + for i in &mut speakers { + i.run(&card, &buf); + } } - - } diff --git a/src/types.rs b/src/types.rs index 6bcf0d4..8312967 100644 --- a/src/types.rs +++ b/src/types.rs @@ -2,7 +2,6 @@ // (C) 2022 The Asahi Linux Contributors use std::ffi::{CString, CStr}; -use half::f16; use configparser::ini::Ini; use alsa::ctl::Ctl; @@ -12,8 +11,7 @@ use crate::helpers; Struct with fields necessary for manipulating an ALSA elem. The val field is created using a wrapper so that we can handle - any errors. This is also necessary so that we can create one of type - Bytes for the V/ISENSE elems. + any errors. */ struct Elem { elem_name: String, @@ -53,8 +51,8 @@ impl ALSAElem for Elem { Speaker. Populated with the important ALSA controls at runtime. level: mixer volume control - vsense: VSENSE as reported by the driver (V, readonly) - isense: ISENSE as reported by the driver (A, readonly) + vsense: VSENSE switch + isense: ISENSE switch */ struct Mixer { @@ -67,14 +65,12 @@ struct Mixer { trait ALSACtl { fn new(name: &str, card: &Ctl) -> Self; - fn get_vsense(&mut self, card: &Ctl) -> f16; - fn get_isense(&mut self, card: &Ctl) -> f16; fn get_lvl(&mut self, card: &Ctl) -> f32; fn set_lvl(&mut self, card: &Ctl, lvl: f32); } impl ALSACtl for Mixer { - // TODO: wire up real V/ISENSE elems (pending driver support) + // TODO: implement turning on V/ISENSE fn new(name: &str, card: &Ctl) -> Mixer { let new_mixer: Mixer = { Mixer { drv: name.to_owned(), @@ -89,49 +85,6 @@ impl ALSACtl for Mixer { return new_mixer; } - /** - MOCK IMPLEMENTATIONS - - V/ISENSE are 16-bit floats sent in a 32-bit TDM slot by the codec. - This is expressed by the driver as a byte array, with rightmost 16 - bits as padding. - - TODO: Condense into a single function and pass in a borrowed Elem - */ - fn get_vsense(&mut self, card: &Ctl) -> f16 { - helpers::read_ev(card, &mut self.vsense.val, &self.vsense.elem_name); - let val: &[u8] = match self.vsense.val.get_bytes() { - Some(inner) => inner, - None => { - println!("Could not read VSENSE from {}", self.drv); - helpers::fail(); - std::process::exit(1); - } - }; - - - let vs = f16::from_ne_bytes([val[0], val[1]]); - - return vs; - } - - fn get_isense(&mut self, card: &Ctl) -> f16 { - helpers::read_ev(card, &mut self.isense.val, &self.isense.elem_name); - let val: &[u8] = match self.vsense.val.get_bytes() { - Some(inner) => inner, - None => { - println!("Could not read ISENSE from {}", self.drv); - helpers::fail(); - std::process::exit(1); - } - }; - - - let is = f16::from_ne_bytes([val[0], val[1]]); - - return is; - } - fn get_lvl(&mut self, card: &Ctl) -> f32 { helpers::read_ev(card, &mut self.level.val, &self.level.elem_name); @@ -186,16 +139,19 @@ impl ALSACtl for Mixer { pub struct Speaker { name: String, alsa_iface: Mixer, - tau_coil: f64, - tr_coil: f64, - temp_limit: f64, + tau_coil: f32, + tau_magnet: f32, + tr_coil: f32, + temp_limit: f32, + vs_chan: i64, + is_chan: i64, } pub trait SafetyMonitor { fn new(driver_name: &str, config: &Ini, card: &Ctl) -> Self; - - fn run(&mut self, card: &Ctl); + fn power_now(&mut self, vs: &[i16], is: &[i16]) -> f32; + fn run(&mut self, card: &Ctl, buf: &[i16; 128 * 6 * 2]); } impl SafetyMonitor for Speaker { @@ -204,39 +160,61 @@ impl SafetyMonitor for Speaker { name: driver_name.to_string(), alsa_iface: ALSACtl::new(&driver_name, card), tau_coil: helpers::parse_float(config, driver_name, "tau_coil"), + tau_magnet: helpers::parse_float(config, driver_name, "tau_magnet"), tr_coil: helpers::parse_float(config, driver_name, "tr_coil"), temp_limit: helpers::parse_float(config, driver_name, "temp_limit"), + vs_chan: helpers::parse_int(config, driver_name, "vs_chan"), + is_chan: helpers::parse_int(config, driver_name, "is_chan"), + }}; return new_speaker; } + fn power_now(&mut self, vs: &[i16], is: &[i16]) -> f32 { + let v_avg: f32 = (vs.iter().sum::() as f32 / vs.len() as f32) * (14 / (2 ^ 15)) as f32; + let i_avg: f32 = (is.iter().sum::() as f32 / is.len() as f32) * (14 / (2 ^ 15)) as f32; + + return v_avg * i_avg; + } + // I'm not sure on the maths here for determining when to start dropping the volume. - fn run(&mut self, card: &Ctl) { - //let v: f16 = self.alsa_iface.get_vsense(card); - //let i: f16 = self.alsa_iface.get_isense(card); + fn run(&mut self, card: &Ctl, buf: &[i16; 128 * 6 * 2]) { let lvl: f32 = self.alsa_iface.get_lvl(card); + let vsense = &buf[(128 * self.vs_chan as usize - 1) .. (128 * self.vs_chan as usize - 1) + 128]; + let isense = &buf[(128 * self.is_chan as usize - 1) .. (128 * self.is_chan as usize - 1) + 128]; - // Technically, this is the temp ~tau_coil seconds in the future - //let temp: f64 = ((v * i).to_f64()) * self.tr_coil; + // Estimate temperature of VC and magnet + let temp0: f32 = 35f32; + let mut temp_vc: f32 = temp0; + let mut temp_magnet: f32 = temp0; + let alpha_vc: f32 = 0.01 / (temp_vc + 0.01); + let alpha_magnet: f32 = 0.01 / (temp_magnet + 0.01); - // if temp < self.temp_limit && lvl < 0f32 { - // println!("Voice coil for {} below temp limit, ramping back up.", self.name); - // - // // For every degree below temp_limit, raise level by 0.5 dB - // let new_lvl: f32 = lvl + ((self.temp_limit - temp) as f32 * 0.5); - // self.alsa_iface.set_lvl(card, new_lvl); - // } - // - // if temp > self.temp_limit { - // println!("Voice coil at {}*C in {} on {}! Dropping volume!", temp, self.tau_coil, self.name); - // - // // For every degree above temp_limit, drop the level by 1.5 dB - // let new_lvl: f32 = lvl - ((temp - self.temp_limit) as f32 * 1.5); - // self.alsa_iface.set_lvl(card, new_lvl); - // } + // Power through the voice coil (average of most recent 128 samples) + let pwr: f32 = self.power_now(&vsense, &isense); - // TEMPORARY PROOF THAT THIS WORKS! + let vc_target: f32 = temp_magnet + pwr * self.tau_coil; + temp_vc = vc_target * alpha_vc + temp_vc * (1.0 - alpha_vc); + println!("Current voice coil temp: {:.2}*C", temp_vc); + + let magnet_target: f32 = temp0 + pwr * self.tau_magnet; + temp_magnet = magnet_target * alpha_magnet + temp_magnet * (1.0 - alpha_magnet); + println!("Current magnet temp: {:.2}*C", temp_magnet); + + if temp_vc < self.temp_limit { + println!("Voice coil for {} below temp limit, ramping back up.", self.name); + // For every degree below temp_limit, raise level by 0.5 dB + let new_lvl: f32 = lvl + ((self.temp_limit - temp_vc) as f32 * 0.5); + self.alsa_iface.set_lvl(card, new_lvl); + } + + if temp_vc > (self.temp_limit - 15f32) { + println!("Voice coil at {}*C on {}! Dropping volume!", temp_vc, self.name); + // For every degree above temp_limit, drop the level by 1.5 dB + let new_lvl: f32 = lvl - ((temp_vc - self.temp_limit) as f32 * 1.5); + self.alsa_iface.set_lvl(card, new_lvl); + } println!("Volume on {} is currently {} dB. Setting to -18 dB.", self.name, lvl); @@ -244,7 +222,5 @@ impl SafetyMonitor for Speaker { self.alsa_iface.set_lvl(card, new_lvl); println!("Volume on {} is now {} dB", self.name, self.alsa_iface.get_lvl(card)); - } - }