code.octet-stream.net Git - m17rt/blob - m17app/src/soundmodem.rs

   1 use crate::error::{M17Error, SoundmodemError};
   2 use crate::tnc::{Tnc, TncError};
   3 use m17core::kiss::MAX_FRAME_LEN;
   4 use m17core::modem::{Demodulator, Modulator, ModulatorAction, SoftDemodulator, SoftModulator};
   5 use m17core::tnc::SoftTnc;
   6 use std::collections::VecDeque;
   7 use std::fs::File;
   8 use std::io::{self, ErrorKind, Read, Write};
   9 use std::path::PathBuf;
  10 use std::sync::mpsc::{channel, sync_channel, Receiver, Sender, SyncSender, TryRecvError};
  11 use std::sync::RwLock;
  12 use std::sync::{Arc, Mutex};
  13 use std::time::{Duration, Instant};
  14 use thiserror::Error;
  15
  16 pub struct Soundmodem {
  17     event_tx: SyncSender<SoundmodemEvent>,
  18     kiss_out_rx: Arc<Mutex<Receiver<Arc<[u8]>>>>,
  19     partial_kiss_out: Arc<Mutex<Option<PartialKissOut>>>,
  20     error_handler: ErrorHandlerInternal,
  21 }
  22
  23 impl Soundmodem {
  24     pub fn new<I: InputSource, O: OutputSink, P: Ptt, E: ErrorHandler>(
  25         input: I,
  26         output: O,
  27         ptt: P,
  28         error: E,
  29     ) -> Self {
  30         let (event_tx, event_rx) = sync_channel(128);
  31         let (kiss_out_tx, kiss_out_rx) = sync_channel(128);
  32         let runtime_error_handler: ErrorHandlerInternal = Arc::new(Mutex::new(Box::new(error)));
  33         spawn_soundmodem_worker(
  34             event_tx.clone(),
  35             event_rx,
  36             kiss_out_tx,
  37             Box::new(input),
  38             Box::new(output),
  39             Box::new(ptt),
  40             runtime_error_handler.clone(),
  41         );
  42         Self {
  43             event_tx,
  44             kiss_out_rx: Arc::new(Mutex::new(kiss_out_rx)),
  45             partial_kiss_out: Arc::new(Mutex::new(None)),
  46             error_handler: runtime_error_handler,
  47         }
  48     }
  49 }
  50
  51 #[derive(Debug, Clone, Copy)]
  52 pub enum ErrorSource {
  53     Input,
  54     Output,
  55     Ptt,
  56 }
  57
  58 pub trait ErrorHandler: Send + Sync + 'static {
  59     fn soundmodem_error(&mut self, source: ErrorSource, err: SoundmodemError);
  60 }
  61
  62 impl<F> ErrorHandler for F
  63 where
  64     F: FnMut(ErrorSource, SoundmodemError) + Send + Sync + 'static,
  65 {
  66     fn soundmodem_error(&mut self, source: ErrorSource, err: SoundmodemError) {
  67         self(source, err)
  68     }
  69 }
  70
  71 pub struct NullErrorHandler;
  72
  73 impl NullErrorHandler {
  74     pub fn new() -> Self {
  75         Self {}
  76     }
  77 }
  78
  79 impl Default for NullErrorHandler {
  80     fn default() -> Self {
  81         Self::new()
  82     }
  83 }
  84
  85 impl ErrorHandler for NullErrorHandler {
  86     fn soundmodem_error(&mut self, source: ErrorSource, err: SoundmodemError) {
  87         let _ = source;
  88         let _ = err;
  89     }
  90 }
  91
  92 type ErrorHandlerInternal = Arc<Mutex<Box<dyn ErrorHandler>>>;
  93
  94 #[derive(Clone)]
  95 pub struct SoundmodemErrorSender {
  96     source: ErrorSource,
  97     event_tx: SyncSender<SoundmodemEvent>,
  98 }
  99
 100 impl SoundmodemErrorSender {
 101     pub fn send_error<E: Into<SoundmodemError>>(&self, err: E) {
 102         let _ = self
 103             .event_tx
 104             .send(SoundmodemEvent::RuntimeError(self.source, err.into()));
 105     }
 106 }
 107
 108 struct PartialKissOut {
 109     output: Arc<[u8]>,
 110     idx: usize,
 111 }
 112
 113 impl Read for Soundmodem {
 114     fn read(&mut self, buf: &mut [u8]) -> io::Result<usize> {
 115         {
 116             let mut partial_kiss_out = self.partial_kiss_out.lock().unwrap();
 117             if let Some(partial) = partial_kiss_out.as_mut() {
 118                 let remaining = partial.output.len() - partial.idx;
 119                 let to_write = remaining.min(buf.len());
 120                 buf[0..to_write]
 121                     .copy_from_slice(&partial.output[partial.idx..(partial.idx + to_write)]);
 122                 if to_write == remaining {
 123                     *partial_kiss_out = None;
 124                 } else {
 125                     partial.idx += to_write;
 126                 }
 127                 return Ok(to_write);
 128             }
 129         }
 130         let output = {
 131             let rx = self.kiss_out_rx.lock().unwrap();
 132             rx.recv()
 133                 .map_err(|s| io::Error::new(ErrorKind::Other, format!("{:?}", s)))?
 134         };
 135         let to_write = output.len().min(buf.len());
 136         buf[0..to_write].copy_from_slice(&output[0..to_write]);
 137         if to_write != output.len() {
 138             *self.partial_kiss_out.lock().unwrap() = Some(PartialKissOut {
 139                 output,
 140                 idx: to_write,
 141             })
 142         }
 143         Ok(to_write)
 144     }
 145 }
 146
 147 impl Write for Soundmodem {
 148     fn write(&mut self, buf: &[u8]) -> std::io::Result<usize> {
 149         let _ = self.event_tx.try_send(SoundmodemEvent::Kiss(buf.into()));
 150         Ok(buf.len())
 151     }
 152
 153     fn flush(&mut self) -> std::io::Result<()> {
 154         Ok(())
 155     }
 156 }
 157
 158 impl Tnc for Soundmodem {
 159     fn try_clone(&mut self) -> Result<Self, TncError> {
 160         Ok(Self {
 161             event_tx: self.event_tx.clone(),
 162             kiss_out_rx: self.kiss_out_rx.clone(),
 163             partial_kiss_out: self.partial_kiss_out.clone(),
 164             error_handler: self.error_handler.clone(),
 165         })
 166     }
 167
 168     fn start(&mut self) {
 169         let _ = self.event_tx.send(SoundmodemEvent::Start);
 170     }
 171
 172     fn close(&mut self) {
 173         let _ = self.event_tx.send(SoundmodemEvent::Close);
 174     }
 175 }
 176
 177 pub enum SoundmodemEvent {
 178     Kiss(Arc<[u8]>),
 179     BasebandInput(Arc<[i16]>),
 180     Start,
 181     Close,
 182     DidReadFromOutputBuffer { len: usize, timestamp: Instant },
 183     OutputUnderrun,
 184     RuntimeError(ErrorSource, SoundmodemError),
 185 }
 186
 187 fn spawn_soundmodem_worker(
 188     event_tx: SyncSender<SoundmodemEvent>,
 189     event_rx: Receiver<SoundmodemEvent>,
 190     kiss_out_tx: SyncSender<Arc<[u8]>>,
 191     input: Box<dyn InputSource>,
 192     output: Box<dyn OutputSink>,
 193     mut ptt_driver: Box<dyn Ptt>,
 194     error_handler: ErrorHandlerInternal,
 195 ) {
 196     std::thread::spawn(move || {
 197         // TODO: should be able to provide a custom Demodulator for a soundmodem
 198         let mut demodulator = SoftDemodulator::new();
 199         let mut modulator = SoftModulator::new();
 200         let mut tnc = SoftTnc::new();
 201         let mut buf = [0u8; MAX_FRAME_LEN];
 202         let out_buffer = Arc::new(RwLock::new(OutputBuffer::new()));
 203         let mut out_samples = [0i16; 1024];
 204         let start = Instant::now();
 205         let mut ptt = false;
 206         while let Ok(ev) = event_rx.recv() {
 207             // Update clock on TNC before we do anything
 208             let sample_time = start.elapsed();
 209             let secs = sample_time.as_secs();
 210             let nanos = sample_time.subsec_nanos();
 211             // Accurate to within approx 1 sample
 212             let now_samples = 48000 * secs + (nanos as u64 / 20833);
 213             tnc.set_now(now_samples);
 214
 215             // Handle event
 216             match ev {
 217                 SoundmodemEvent::Kiss(k) => {
 218                     let _n = tnc.write_kiss(&k);
 219                     // TODO: what does it mean if we fail to write it all?
 220                     // Probably we have to read frames for tx first - revisit this during tx
 221                 }
 222                 SoundmodemEvent::BasebandInput(b) => {
 223                     for sample in &*b {
 224                         if let Some(frame) = demodulator.demod(*sample) {
 225                             tnc.handle_frame(frame);
 226                             loop {
 227                                 let n = tnc.read_kiss(&mut buf);
 228                                 if n > 0 {
 229                                     let _ = kiss_out_tx.try_send(buf[0..n].into());
 230                                 } else {
 231                                     break;
 232                                 }
 233                             }
 234                         }
 235                     }
 236                     tnc.set_data_carrier_detect(demodulator.data_carrier_detect());
 237                 }
 238                 SoundmodemEvent::Start => {
 239                     let input_errors = SoundmodemErrorSender {
 240                         source: ErrorSource::Input,
 241                         event_tx: event_tx.clone(),
 242                     };
 243                     input.start(event_tx.clone(), input_errors);
 244                     let output_errors = SoundmodemErrorSender {
 245                         source: ErrorSource::Output,
 246                         event_tx: event_tx.clone(),
 247                     };
 248                     output.start(event_tx.clone(), out_buffer.clone(), output_errors);
 249                 }
 250                 SoundmodemEvent::Close => {
 251                     input.close();
 252                     output.close();
 253                     if let Err(e) = ptt_driver.ptt_off() {
 254                         error_handler
 255                             .lock()
 256                             .unwrap()
 257                             .soundmodem_error(ErrorSource::Ptt, e);
 258                     }
 259                     break;
 260                 }
 261                 SoundmodemEvent::DidReadFromOutputBuffer { len, timestamp } => {
 262                     let (occupied, internal_latency) = {
 263                         let out_buffer = out_buffer.read().unwrap();
 264                         (out_buffer.samples.len(), out_buffer.latency)
 265                     };
 266                     let internal_latency = (internal_latency.as_secs_f32() * 48000.0) as usize;
 267                     let dynamic_latency =
 268                         len.saturating_sub((timestamp.elapsed().as_secs_f32() * 48000.0) as usize);
 269                     modulator.update_output_buffer(
 270                         occupied,
 271                         48000,
 272                         internal_latency + dynamic_latency,
 273                     );
 274                 }
 275                 SoundmodemEvent::OutputUnderrun => {
 276                     log::debug!("output underrun");
 277                     // TODO: cancel transmission, send empty data frame to host
 278                 }
 279                 SoundmodemEvent::RuntimeError(source, err) => {
 280                     error_handler.lock().unwrap().soundmodem_error(source, err);
 281                 }
 282             }
 283
 284             // Update PTT state
 285             let new_ptt = tnc.ptt();
 286             if new_ptt != ptt {
 287                 if new_ptt {
 288                     if let Err(e) = ptt_driver.ptt_on() {
 289                         error_handler
 290                             .lock()
 291                             .unwrap()
 292                             .soundmodem_error(ErrorSource::Ptt, e);
 293                     }
 294                 } else if let Err(e) = ptt_driver.ptt_off() {
 295                     error_handler
 296                         .lock()
 297                         .unwrap()
 298                         .soundmodem_error(ErrorSource::Ptt, e);
 299                 }
 300             }
 301             ptt = new_ptt;
 302
 303             // Let the modulator do what it wants
 304             while let Some(action) = modulator.run() {
 305                 match action {
 306                     ModulatorAction::SetIdle(idling) => {
 307                         out_buffer.write().unwrap().idling = idling;
 308                     }
 309                     ModulatorAction::GetNextFrame => {
 310                         modulator.provide_next_frame(tnc.read_tx_frame());
 311                     }
 312                     ModulatorAction::ReadOutput => loop {
 313                         let n = modulator.read_output_samples(&mut out_samples);
 314                         if n == 0 {
 315                             break;
 316                         }
 317                         let mut out_buffer = out_buffer.write().unwrap();
 318                         for s in &out_samples[0..n] {
 319                             out_buffer.samples.push_back(*s);
 320                         }
 321                     },
 322                     ModulatorAction::TransmissionWillEnd(in_samples) => {
 323                         tnc.set_tx_end_time(in_samples);
 324                     }
 325                 }
 326             }
 327         }
 328     });
 329 }
 330
 331 pub trait InputSource: Send + Sync + 'static {
 332     fn start(&self, samples: SyncSender<SoundmodemEvent>, errors: SoundmodemErrorSender);
 333     fn close(&self);
 334 }
 335
 336 pub struct InputRrcFile {
 337     baseband: Arc<[u8]>,
 338     end_tx: Mutex<Option<Sender<()>>>,
 339 }
 340
 341 impl InputRrcFile {
 342     pub fn new(path: PathBuf) -> Result<Self, M17Error> {
 343         let mut file = File::open(&path).map_err(|_| M17Error::InvalidRrcPath(path.clone()))?;
 344         let mut baseband = vec![];
 345         file.read_to_end(&mut baseband)
 346             .map_err(|_| M17Error::RrcReadFailed(path))?;
 347         Ok(Self {
 348             baseband: baseband.into(),
 349             end_tx: Mutex::new(None),
 350         })
 351     }
 352 }
 353
 354 impl InputSource for InputRrcFile {
 355     fn start(&self, samples: SyncSender<SoundmodemEvent>, errors: SoundmodemErrorSender) {
 356         let (end_tx, end_rx) = channel();
 357         let baseband = self.baseband.clone();
 358         std::thread::spawn(move || {
 359             // assuming 48 kHz for now
 360             const TICK: Duration = Duration::from_millis(25);
 361             const SAMPLES_PER_TICK: usize = 1200;
 362
 363             let mut next_tick = Instant::now() + TICK;
 364             let mut buf = [0i16; SAMPLES_PER_TICK];
 365             let mut idx = 0;
 366
 367             for sample in baseband
 368                 .chunks(2)
 369                 .map(|pair| i16::from_le_bytes([pair[0], pair[1]]))
 370             {
 371                 buf[idx] = sample;
 372                 idx += 1;
 373                 if idx == SAMPLES_PER_TICK {
 374                     if samples
 375                         .try_send(SoundmodemEvent::BasebandInput(buf.into()))
 376                         .is_err()
 377                     {
 378                         errors.send_error(InputRrcError::Overflow);
 379                     }
 380                     next_tick += TICK;
 381                     idx = 0;
 382                     std::thread::sleep(next_tick.duration_since(Instant::now()));
 383                 }
 384                 if end_rx.try_recv() != Err(TryRecvError::Empty) {
 385                     break;
 386                 }
 387             }
 388         });
 389         *self.end_tx.lock().unwrap() = Some(end_tx);
 390     }
 391
 392     fn close(&self) {
 393         let _ = self.end_tx.lock().unwrap().take();
 394     }
 395 }
 396
 397 #[derive(Debug, Error)]
 398 pub enum InputRrcError {
 399     #[error("overflow occurred feeding sample to soundmodem")]
 400     Overflow,
 401 }
 402
 403 pub struct NullInputSource {
 404     end_tx: Mutex<Option<Sender<()>>>,
 405 }
 406
 407 impl NullInputSource {
 408     pub fn new() -> Self {
 409         Self {
 410             end_tx: Mutex::new(None),
 411         }
 412     }
 413 }
 414
 415 impl InputSource for NullInputSource {
 416     fn start(&self, samples: SyncSender<SoundmodemEvent>, errors: SoundmodemErrorSender) {
 417         let (end_tx, end_rx) = channel();
 418         std::thread::spawn(move || {
 419             // assuming 48 kHz for now
 420             const TICK: Duration = Duration::from_millis(25);
 421             const SAMPLES_PER_TICK: usize = 1200;
 422             let mut next_tick = Instant::now() + TICK;
 423
 424             loop {
 425                 std::thread::sleep(next_tick.duration_since(Instant::now()));
 426                 next_tick += TICK;
 427                 if end_rx.try_recv() != Err(TryRecvError::Empty) {
 428                     break;
 429                 }
 430                 if samples
 431                     .try_send(SoundmodemEvent::BasebandInput(
 432                         [0i16; SAMPLES_PER_TICK].into(),
 433                     ))
 434                     .is_err()
 435                 {
 436                     errors.send_error(NullInputError::Overflow);
 437                 }
 438             }
 439         });
 440         *self.end_tx.lock().unwrap() = Some(end_tx);
 441     }
 442
 443     fn close(&self) {
 444         let _ = self.end_tx.lock().unwrap().take();
 445     }
 446 }
 447
 448 #[derive(Debug, Error)]
 449 pub enum NullInputError {
 450     #[error("overflow occurred feeding sample to soundmodem")]
 451     Overflow,
 452 }
 453
 454 impl Default for NullInputSource {
 455     fn default() -> Self {
 456         Self::new()
 457     }
 458 }
 459
 460 pub struct OutputBuffer {
 461     pub idling: bool,
 462     // TODO: something more efficient
 463     pub samples: VecDeque<i16>,
 464     pub latency: Duration,
 465 }
 466
 467 impl OutputBuffer {
 468     pub fn new() -> Self {
 469         Self {
 470             idling: true,
 471             samples: VecDeque::new(),
 472             latency: Duration::ZERO,
 473         }
 474     }
 475 }
 476
 477 impl Default for OutputBuffer {
 478     fn default() -> Self {
 479         Self::new()
 480     }
 481 }
 482
 483 pub trait OutputSink: Send + Sync + 'static {
 484     fn start(
 485         &self,
 486         event_tx: SyncSender<SoundmodemEvent>,
 487         buffer: Arc<RwLock<OutputBuffer>>,
 488         errors: SoundmodemErrorSender,
 489     );
 490     fn close(&self);
 491 }
 492
 493 pub struct OutputRrcFile {
 494     path: PathBuf,
 495     end_tx: Mutex<Option<Sender<()>>>,
 496 }
 497
 498 impl OutputRrcFile {
 499     pub fn new(path: PathBuf) -> Self {
 500         Self {
 501             path,
 502             end_tx: Mutex::new(None),
 503         }
 504     }
 505 }
 506
 507 impl OutputSink for OutputRrcFile {
 508     fn start(
 509         &self,
 510         event_tx: SyncSender<SoundmodemEvent>,
 511         buffer: Arc<RwLock<OutputBuffer>>,
 512         errors: SoundmodemErrorSender,
 513     ) {
 514         let (end_tx, end_rx) = channel();
 515         let mut file = match File::create(self.path.clone()) {
 516             Ok(f) => f,
 517             Err(e) => {
 518                 errors.send_error(OutputRrcError::Open(e));
 519                 return;
 520             }
 521         };
 522         std::thread::spawn(move || {
 523             // assuming 48 kHz for now
 524             const TICK: Duration = Duration::from_millis(25);
 525             const SAMPLES_PER_TICK: usize = 1200;
 526
 527             // flattened BE i16s for writing
 528             let mut buf = [0u8; SAMPLES_PER_TICK * 2];
 529             let mut next_tick = Instant::now() + TICK;
 530
 531             loop {
 532                 std::thread::sleep(next_tick.duration_since(Instant::now()));
 533                 next_tick += TICK;
 534                 if end_rx.try_recv() != Err(TryRecvError::Empty) {
 535                     break;
 536                 }
 537                 // For now only write deliberately modulated (non-idling) samples
 538                 // Multiple transmissions will get smooshed together
 539                 let mut buf_used = 0;
 540
 541                 let mut buffer = buffer.write().unwrap();
 542                 for out in buf.chunks_mut(2) {
 543                     if let Some(s) = buffer.samples.pop_front() {
 544                         let be = s.to_le_bytes();
 545                         out.copy_from_slice(&[be[0], be[1]]);
 546                         buf_used += 2;
 547                     } else if !buffer.idling {
 548                         let _ = event_tx.send(SoundmodemEvent::OutputUnderrun);
 549                         break;
 550                     }
 551                 }
 552                 if let Err(e) = file.write_all(&buf[0..buf_used]) {
 553                     errors.send_error(OutputRrcError::WriteError(e));
 554                     break;
 555                 }
 556                 let _ = event_tx.send(SoundmodemEvent::DidReadFromOutputBuffer {
 557                     len: buf_used / 2,
 558                     timestamp: Instant::now(),
 559                 });
 560             }
 561         });
 562         *self.end_tx.lock().unwrap() = Some(end_tx);
 563     }
 564
 565     fn close(&self) {
 566         let _ = self.end_tx.lock().unwrap().take();
 567     }
 568 }
 569
 570 #[derive(Debug, Error)]
 571 pub enum OutputRrcError {
 572     #[error("unable to open rrc file for writing: {0}")]
 573     Open(#[source] std::io::Error),
 574
 575     #[error("error writing to output file: {0}")]
 576     WriteError(#[source] std::io::Error),
 577 }
 578
 579 pub struct NullOutputSink {
 580     end_tx: Mutex<Option<Sender<()>>>,
 581 }
 582
 583 impl NullOutputSink {
 584     pub fn new() -> Self {
 585         Self {
 586             end_tx: Mutex::new(None),
 587         }
 588     }
 589 }
 590
 591 impl Default for NullOutputSink {
 592     fn default() -> Self {
 593         Self::new()
 594     }
 595 }
 596
 597 impl OutputSink for NullOutputSink {
 598     fn start(
 599         &self,
 600         event_tx: SyncSender<SoundmodemEvent>,
 601         buffer: Arc<RwLock<OutputBuffer>>,
 602         _errors: SoundmodemErrorSender,
 603     ) {
 604         let (end_tx, end_rx) = channel();
 605         std::thread::spawn(move || {
 606             // assuming 48 kHz for now
 607             const TICK: Duration = Duration::from_millis(25);
 608             const SAMPLES_PER_TICK: usize = 1200;
 609             let mut next_tick = Instant::now() + TICK;
 610
 611             loop {
 612                 std::thread::sleep(next_tick.duration_since(Instant::now()));
 613                 next_tick += TICK;
 614                 if end_rx.try_recv() != Err(TryRecvError::Empty) {
 615                     break;
 616                 }
 617
 618                 let mut buffer = buffer.write().unwrap();
 619                 let mut taken = 0;
 620                 for _ in 0..SAMPLES_PER_TICK {
 621                     if buffer.samples.pop_front().is_none() {
 622                         if !buffer.idling {
 623                             let _ = event_tx.send(SoundmodemEvent::OutputUnderrun);
 624                             break;
 625                         }
 626                     } else {
 627                         taken += 1;
 628                     }
 629                 }
 630                 let _ = event_tx.send(SoundmodemEvent::DidReadFromOutputBuffer {
 631                     len: taken,
 632                     timestamp: Instant::now(),
 633                 });
 634             }
 635         });
 636         *self.end_tx.lock().unwrap() = Some(end_tx);
 637     }
 638
 639     fn close(&self) {
 640         let _ = self.end_tx.lock().unwrap().take();
 641     }
 642 }
 643
 644 pub trait Ptt: Send + 'static {
 645     fn ptt_on(&mut self) -> Result<(), SoundmodemError>;
 646     fn ptt_off(&mut self) -> Result<(), SoundmodemError>;
 647 }
 648
 649 /// There is no PTT because this TNC will never make transmissions on a real radio.
 650 pub struct NullPtt;
 651
 652 impl NullPtt {
 653     pub fn new() -> Self {
 654         Self
 655     }
 656 }
 657
 658 impl Default for NullPtt {
 659     fn default() -> Self {
 660         Self::new()
 661     }
 662 }
 663
 664 impl Ptt for NullPtt {
 665     fn ptt_on(&mut self) -> Result<(), SoundmodemError> {
 666         Ok(())
 667     }
 668
 669     fn ptt_off(&mut self) -> Result<(), SoundmodemError> {
 670         Ok(())
 671     }
 672 }