code.octet-stream.net Git - m17rt/blob - m17codec2/src/lib.rs

   1 #![doc = include_str!("../README.md")]
   2
   3 use codec2::{Codec2, Codec2Mode};
   4 use cpal::traits::DeviceTrait;
   5 use cpal::traits::HostTrait;
   6 use cpal::traits::StreamTrait;
   7 use cpal::{Sample, SampleFormat, SampleRate};
   8 use log::debug;
   9 use m17app::adapter::StreamAdapter;
  10 use m17app::app::TxHandle;
  11 use m17app::error::AdapterError;
  12 use m17app::link_setup::LinkSetup;
  13 use m17app::link_setup::M17Address;
  14 use m17app::StreamFrame;
  15 use rubato::Resampler;
  16 use rubato::SincFixedIn;
  17 use rubato::SincInterpolationParameters;
  18 use std::collections::VecDeque;
  19 use std::fs::File;
  20 use std::io::Write;
  21 use std::path::Path;
  22 use std::path::PathBuf;
  23 use std::sync::{
  24     mpsc::{channel, Receiver, Sender},
  25     Arc, Mutex,
  26 };
  27 use std::time::Duration;
  28 use std::time::Instant;
  29 use thiserror::Error;
  30
  31 pub mod soundcards;
  32
  33 /// Write one or more 8-byte chunks of 3200-bit Codec2 to a raw S16LE file
  34 /// and return the samples.
  35 pub fn decode_codec2<P: AsRef<Path>>(data: &[u8], out_path: P) -> Vec<i16> {
  36     let codec2 = Codec2::new(Codec2Mode::MODE_3200);
  37     let var_name = codec2;
  38     let mut codec = var_name;
  39     let mut all_samples: Vec<i16> = vec![];
  40     for i in 0..(data.len() / 8) {
  41         let mut samples = vec![0; codec.samples_per_frame()];
  42         codec.decode(&mut samples, &data[i * 8..((i + 1) * 8)]);
  43         all_samples.append(&mut samples);
  44     }
  45     let mut speech_out = File::create(out_path).unwrap();
  46     for b in &all_samples {
  47         speech_out.write_all(&b.to_le_bytes()).unwrap();
  48     }
  49     all_samples
  50 }
  51
  52 /// Subscribes to M17 streams and attempts to play the decoded Codec2
  53 pub struct Codec2Adapter {
  54     state: Arc<Mutex<AdapterState>>,
  55     output_card: Option<String>,
  56 }
  57
  58 impl Codec2Adapter {
  59     pub fn new() -> Self {
  60         Self {
  61             state: Arc::new(Mutex::new(AdapterState {
  62                 tx: None,
  63                 out_buf: VecDeque::new(),
  64                 codec2: Codec2::new(Codec2Mode::MODE_3200),
  65                 end_tx: None,
  66                 resampler: None,
  67             })),
  68             output_card: None,
  69         }
  70     }
  71
  72     pub fn set_output_card<S: Into<String>>(&mut self, card_name: S) {
  73         self.output_card = Some(card_name.into());
  74     }
  75 }
  76
  77 impl Default for Codec2Adapter {
  78     fn default() -> Self {
  79         Self::new()
  80     }
  81 }
  82
  83 struct AdapterState {
  84     tx: Option<TxHandle>,
  85     /// Circular buffer of output samples for playback
  86     out_buf: VecDeque<i16>,
  87     codec2: Codec2,
  88     end_tx: Option<Sender<()>>,
  89     resampler: Option<SincFixedIn<f32>>,
  90 }
  91
  92 impl StreamAdapter for Codec2Adapter {
  93     fn start(&self, handle: TxHandle) -> Result<(), AdapterError> {
  94         self.state.lock().unwrap().tx = Some(handle);
  95
  96         let (end_tx, end_rx) = channel();
  97         let (setup_tx, setup_rx) = channel();
  98         let state = self.state.clone();
  99         let output_card = self.output_card.clone();
 100         std::thread::spawn(move || stream_thread(end_rx, setup_tx, state, output_card));
 101         self.state.lock().unwrap().end_tx = Some(end_tx);
 102         // Propagate any errors arising in the thread
 103         let sample_rate = setup_rx.recv()??;
 104         debug!("selected codec2 output sample rate {sample_rate}");
 105         if sample_rate != 8000 {
 106             let params = SincInterpolationParameters {
 107                 sinc_len: 256,
 108                 f_cutoff: 0.95,
 109                 oversampling_factor: 256,
 110                 interpolation: rubato::SincInterpolationType::Cubic,
 111                 window: rubato::WindowFunction::BlackmanHarris2,
 112             };
 113             // TODO: fix unwrap
 114             self.state.lock().unwrap().resampler =
 115                 Some(SincFixedIn::new(sample_rate as f64 / 8000f64, 1.0, params, 160, 1).unwrap());
 116         }
 117         Ok(())
 118     }
 119
 120     fn close(&self) -> Result<(), AdapterError> {
 121         let mut state = self.state.lock().unwrap();
 122         state.tx = None;
 123         state.end_tx = None;
 124         Ok(())
 125     }
 126
 127     fn stream_began(&self, _link_setup: LinkSetup) {
 128         // for now we will assume:
 129         // - unencrypted
 130         // - data type is Voice (Codec2 3200), not Voice+Data
 131         // TODO: is encryption handled here or in M17App, such that we get a decrypted stream?
 132         // TODO: handle the Voice+Data combination with Codec2 1600
 133         self.state.lock().unwrap().codec2 = Codec2::new(Codec2Mode::MODE_3200);
 134     }
 135
 136     fn stream_data(&self, _frame_number: u16, _is_final: bool, data: Arc<[u8; 16]>) {
 137         let mut state = self.state.lock().unwrap();
 138         for encoded in data.chunks(8) {
 139             if state.out_buf.len() < 8192 {
 140                 let mut samples = [i16::EQUILIBRIUM; 160]; // while assuming 3200
 141                 state.codec2.decode(&mut samples, encoded);
 142                 if let Some(resampler) = state.resampler.as_mut() {
 143                     let samples_f: Vec<f32> =
 144                         samples.iter().map(|s| *s as f32 / 16384.0f32).collect();
 145                     let res = resampler.process(&vec![samples_f], None).unwrap();
 146                     for s in &res[0] {
 147                         state.out_buf.push_back((s * 16383.0f32) as i16);
 148                     }
 149                 } else {
 150                     // TODO: maybe get rid of VecDeque so we can decode directly into ring buffer?
 151                     for s in samples {
 152                         state.out_buf.push_back(s);
 153                     }
 154                 }
 155             } else {
 156                 debug!("out_buf overflow");
 157             }
 158         }
 159     }
 160 }
 161
 162 fn output_cb(data: &mut [i16], state: &Mutex<AdapterState>, channels: u16) {
 163     let mut state = state.lock().unwrap();
 164     for d in data.chunks_mut(channels as usize) {
 165         d.fill(state.out_buf.pop_front().unwrap_or(i16::EQUILIBRIUM));
 166     }
 167 }
 168
 169 /// Create and manage the stream from a dedicated thread since it's `!Send`
 170 fn stream_thread(
 171     end: Receiver<()>,
 172     setup_tx: Sender<Result<u32, AdapterError>>,
 173     state: Arc<Mutex<AdapterState>>,
 174     output_card: Option<String>,
 175 ) {
 176     let host = cpal::default_host();
 177     let device = if let Some(output_card) = output_card {
 178         // TODO: more error handling for unwraps
 179         match host
 180             .output_devices()
 181             .unwrap()
 182             .find(|d| d.name().unwrap() == output_card)
 183         {
 184             Some(d) => d,
 185             None => {
 186                 let _ = setup_tx.send(Err(M17Codec2Error::CardUnavailable(output_card).into()));
 187                 return;
 188             }
 189         }
 190     } else {
 191         match host.default_output_device() {
 192             Some(d) => d,
 193             None => {
 194                 let _ = setup_tx.send(Err(M17Codec2Error::DefaultCardUnavailable.into()));
 195                 return;
 196             }
 197         }
 198     };
 199     let card_name = device.name().unwrap();
 200     let mut configs = match device.supported_output_configs() {
 201         Ok(c) => c,
 202         Err(e) => {
 203             let _ = setup_tx.send(Err(
 204                 M17Codec2Error::OutputConfigsUnavailable(card_name, e).into()
 205             ));
 206             return;
 207         }
 208     };
 209     let config = match configs.find(|c| {
 210         (c.channels() == 1 || c.channels() == 2) && c.sample_format() == SampleFormat::I16
 211     }) {
 212         Some(c) => c,
 213         None => {
 214             let _ = setup_tx.send(Err(
 215                 M17Codec2Error::SupportedOutputUnavailable(card_name).into()
 216             ));
 217             return;
 218         }
 219     };
 220
 221     let target_sample_rate =
 222         if config.min_sample_rate().0 <= 8000 && config.max_sample_rate().0 >= 8000 {
 223             8000
 224         } else {
 225             config.max_sample_rate().0
 226         };
 227     let channels = config.channels();
 228
 229     let config = config.with_sample_rate(SampleRate(target_sample_rate));
 230     let stream = match device.build_output_stream(
 231         &config.into(),
 232         move |data: &mut [i16], _info: &cpal::OutputCallbackInfo| {
 233             output_cb(data, &state, channels);
 234         },
 235         |e| {
 236             // trigger end_tx here? always more edge cases
 237             debug!("error occurred in codec2 playback: {e:?}");
 238         },
 239         None,
 240     ) {
 241         Ok(s) => s,
 242         Err(e) => {
 243             let _ = setup_tx.send(Err(
 244                 M17Codec2Error::OutputStreamBuildError(card_name, e).into()
 245             ));
 246             return;
 247         }
 248     };
 249     match stream.play() {
 250         Ok(()) => (),
 251         Err(e) => {
 252             let _ = setup_tx.send(Err(
 253                 M17Codec2Error::OutputStreamPlayError(card_name, e).into()
 254             ));
 255             return;
 256         }
 257     }
 258     let _ = setup_tx.send(Ok(target_sample_rate));
 259     let _ = end.recv();
 260     // it seems concrete impls of Stream have a Drop implementation that will handle termination
 261 }
 262
 263 /// Transmits a wave file as an M17 stream
 264 pub struct WavePlayer;
 265
 266 impl WavePlayer {
 267     /// Plays a wave file (blocking).
 268     ///
 269     /// * `path`: wave file to transmit, must be 8 kHz mono and 16-bit LE
 270     /// * `tx`: a `TxHandle` obtained from an `M17App`
 271     /// * `source`: address of transmission source
 272     /// * `destination`: address of transmission destination
 273     /// * `channel_access_number`: from 0 to 15, usually 0
 274     pub fn play(
 275         path: PathBuf,
 276         tx: TxHandle,
 277         source: &M17Address,
 278         destination: &M17Address,
 279         channel_access_number: u8,
 280     ) {
 281         let mut reader = hound::WavReader::open(path).unwrap();
 282         let mut samples = reader.samples::<i16>();
 283
 284         let mut codec = Codec2::new(Codec2Mode::MODE_3200);
 285         let mut in_buf = [0i16; 160];
 286         let mut out_buf = [0u8; 16];
 287         let mut lsf_chunk: usize = 0;
 288         const TICK: Duration = Duration::from_millis(40);
 289         let mut next_tick = Instant::now() + TICK;
 290         let mut frame_number = 0;
 291
 292         let mut setup = LinkSetup::new_voice(source, destination);
 293         setup.set_channel_access_number(channel_access_number);
 294         tx.transmit_stream_start(&setup);
 295
 296         loop {
 297             let mut last_one = false;
 298             for out in out_buf.chunks_mut(8) {
 299                 for i in in_buf.iter_mut() {
 300                     let sample = match samples.next() {
 301                         Some(Ok(sample)) => sample,
 302                         _ => {
 303                             last_one = true;
 304                             0
 305                         }
 306                     };
 307                     *i = sample;
 308                 }
 309                 codec.encode(out, &in_buf);
 310             }
 311             tx.transmit_stream_next(&StreamFrame {
 312                 lich_idx: lsf_chunk as u8,
 313                 lich_part: setup.lich_part(lsf_chunk as u8),
 314                 frame_number,
 315                 end_of_stream: last_one,
 316                 stream_data: out_buf,
 317             });
 318             frame_number += 1;
 319             lsf_chunk = (lsf_chunk + 1) % 6;
 320
 321             if last_one {
 322                 break;
 323             }
 324
 325             std::thread::sleep(next_tick.duration_since(Instant::now()));
 326             next_tick += TICK;
 327         }
 328     }
 329 }
 330
 331 #[derive(Debug, Error)]
 332 pub enum M17Codec2Error {
 333     #[error("selected card '{0}' does not exist or is in use")]
 334     CardUnavailable(String),
 335
 336     #[error("default output card is unavailable")]
 337     DefaultCardUnavailable,
 338
 339     #[error("selected card '{0}' failed to list available output configs: '{1}'")]
 340     OutputConfigsUnavailable(String, #[source] cpal::SupportedStreamConfigsError),
 341
 342     #[error("selected card '{0}' did not offer a compatible output config type, either due to hardware limitations or because it is currently in use")]
 343     SupportedOutputUnavailable(String),
 344
 345     #[error("selected card '{0}' was unable to build an output stream: '{1}'")]
 346     OutputStreamBuildError(String, #[source] cpal::BuildStreamError),
 347
 348     #[error("selected card '{0}' was unable to play an output stream: '{1}'")]
 349     OutputStreamPlayError(String, #[source] cpal::PlayStreamError),
 350 }