RTSPAudioStreamer.h

/*
 * Author: Phil Schatzmann
 *
 * Based on Micro-RTSP library:
 * https://github.com/Tomp0801/Micro-RTSP-Audio
 * https://github.com/geeksville/Micro-RTSP
 *
 */
 
#pragma once
 
#include <stdio.h>
 
#ifdef __linux__
#include "AudioTools/Concurrency/Desktop.h"
#else
#include "AudioTools/Concurrency/RTOS.h"
#endif
#include "AudioTools/CoreAudio/AudioBasic/Collections/Vector.h"
#include "AudioTools/CoreAudio/AudioTimer.h"
#include "IAudioSource.h"
#include "RTSPPlatform.h"
 
namespace audio_tools {
 
template <typename Platform>
class RTSPAudioStreamerBase {
 public:
  RTSPAudioStreamerBase() {
    LOGD("Creating RTSP Audio streamer base");
    m_RtpServerPort = 0;
    m_RtcpServerPort = 0;
 
    m_SequenceNumber = 0;
    m_Timestamp = 0;
    m_SendIdx = 0;
 
    m_RtpSocket = Platform::NULL_UDP_SOCKET;
    m_RtcpSocket = Platform::NULL_UDP_SOCKET;
 
    m_prevMsec = 0;
 
    m_udpRefCount = 0;
 
    m_useTcpInterleaved = false;
    m_RtspTcpSocket = Platform::NULL_TCP_SOCKET;
    m_TcpRtpChannel = 0;
    m_TcpRtcpChannel = 1;
 
    m_lastSamplesSent = 0;
  }
 
  RTSPAudioStreamerBase(IAudioSource &source) : RTSPAudioStreamerBase() {
    setAudioSource(&source);
  }
 
  virtual ~RTSPAudioStreamerBase() {
    // mRtpBuf is automatically managed by Vector
  }
 
  virtual void setAudioSource(IAudioSource *source) {
    m_audioSource = source;
    initAudioSource();
    LOGI("RTSP Audio streamer created.  Fragment size: %i bytes",
         m_fragmentSize);
  }
 
  bool initAudioSource() {
    LOGI("initAudioSource");
    if (getAudioSource() == nullptr) {
      LOGE("audio_source is null");
      return false;
    }
    RTSPFormat &fmt = getAudioSource()->getFormat();
    m_payloadType = fmt.rtpPayloadType();
    m_fragmentSize = fmt.fragmentSize();
    m_timer_period_us = fmt.timerPeriodUs();
    LOGI("m_fragmentSize (bytes): %d", m_fragmentSize);
    return true;
  }
 
  bool initUdpTransport(IPAddress aClientIP, uint16_t aClientPort) {
    m_ClientIP = aClientIP;
    m_ClientPort = aClientPort;
 
    m_SequenceNumber = random(65536);
 
    if (m_udpRefCount != 0) {
      ++m_udpRefCount;
      return true;
    }
 
    for (u_short P = 6970; P < 0xFFFE; P += 2) {
      m_RtpSocket = Platform::createUdpSocket(P);
      if (m_RtpSocket) {  // Rtp socket was bound successfully. Lets try to bind
                          // the consecutive Rtsp socket
        m_RtcpSocket = Platform::createUdpSocket(P + 1);
        if (m_RtcpSocket) {
          m_RtpServerPort = P;
          m_RtcpServerPort = P + 1;
          break;
        } else {
          Platform::closeUdpSocket(m_RtpSocket);
          Platform::closeUdpSocket(m_RtcpSocket);
        };
      }
    };
    ++m_udpRefCount;
 
    LOGI("RTP Streamer set up with client IP %s and client Port %i",
         m_ClientIP.toString().c_str(), m_ClientPort);
 
    // If client IP is unknown (0.0.0.0), try to learn it from an inbound UDP packet
    tryLearnClientFromUdp(true);
 
    return true;
  }
 
  void initTcpInterleavedTransport(typename Platform::TcpClientType *tcpSock,
                                   int rtpChannel, int rtcpChannel) {
    m_RtspTcpSocket = tcpSock;
    m_TcpRtpChannel = rtpChannel;
    m_TcpRtcpChannel = rtcpChannel;
    m_useTcpInterleaved = true;
    m_SequenceNumber = random(65536);
    LOGI("Using RTP over RTSP TCP interleaved: ch=%d/%d", rtpChannel,
         rtcpChannel);
  }
 
  void releaseUdpTransport(void) {
    --m_udpRefCount;
    if (m_udpRefCount == 0) {
      m_RtpServerPort = 0;
      m_RtcpServerPort = 0;
      Platform::closeUdpSocket(m_RtpSocket);
      Platform::closeUdpSocket(m_RtcpSocket);
 
      m_RtpSocket = Platform::NULL_UDP_SOCKET;
      m_RtcpSocket = Platform::NULL_UDP_SOCKET;
    }
  }
 
  int sendRtpPacketDirect() {
    // check buffer
    if (mRtpBuf.size() == 0) {
      LOGE("mRtpBuf is empty");
      return -1;
    }
 
    // append data to header
    if (m_audioSource == nullptr) {
      LOGE("No audio source provided");
      return -1;
    }
 
    // unsigned char * dataBuf = &mRtpBuf[m_fragmentSize];
    if (m_fragmentSize + HEADER_SIZE >= STREAMING_BUFFER_SIZE) {
      LOGE(
          "STREAMIN_BUFFER_SIZE too small for the sampling rate: increase to "
          "%d",
          m_fragmentSize + HEADER_SIZE);
      return -1;
    }
 
    // Generic path (PCM, G711, etc.)
    memset(mRtpBuf.data(), 0, STREAMING_BUFFER_SIZE);
    buildRtpHeader();
 
    unsigned char *dataBuf = &mRtpBuf[HEADER_SIZE];
    int header_len = m_audioSource->getFormat().readHeader(dataBuf);
    int maxPayload = STREAMING_BUFFER_SIZE - HEADER_SIZE - header_len;
    dataBuf += header_len;
 
    int toRead = m_fragmentSize;
    if (toRead > maxPayload) {
      LOGW("Fragment exceeds payload capacity (%d > %d); clamping", toRead, maxPayload);
      toRead = maxPayload;
    }
    int bytesRead = m_audioSource->readBytes((void *)dataBuf, toRead);
    LOGI("Read %d bytes from audio source", bytesRead);
    int bytesNet = m_audioSource->getFormat().convert(dataBuf, bytesRead);
 
    // Compute samples sent for timestamp increment
    m_lastSamplesSent = m_audioSource->getFormat().timestampIncrement();
    m_SequenceNumber++;
    sendOut(HEADER_SIZE + bytesNet + header_len);
    return bytesNet;
  }
 
  virtual void start() {
    LOGI("Starting audio source (base)");
 
    if (mRtpBuf.size() == 0) {
      mRtpBuf.resize(STREAMING_BUFFER_SIZE + 1);
    }
 
    if (m_audioSource != nullptr) {
      initAudioSource();
      m_audioSource->start();
      LOGI("Audio source started - ready for manual streaming");
    } else {
      LOGE("No streaming source");
    }
  }
 
  virtual void stop() {
    LOGI("Stopping audio source (base)");
 
    if (m_audioSource != nullptr) {
      m_audioSource->stop();
    }
 
    LOGI("Audio source stopped");
  }
 
  u_short getRtpServerPort() { return m_RtpServerPort; }
 
  u_short getRtcpServerPort() { return m_RtcpServerPort; }
 
  IAudioSource *getAudioSource() { return m_audioSource; }
 
  uint32_t getTimerPeriodUs() const { return m_timer_period_us; }
 
  uint32_t getTimerPeriodMs() const { return m_timer_period_us / 1000; }
 
  uint16_t currentSeq() const { return m_SequenceNumber; }
 
  uint32_t currentRtpTimestamp() const { return m_Timestamp; }
 
  uint32_t currentSsrc() const { return m_Ssrc; }
 
  bool checkTimerPeriodChange() {
    if (m_audioSource == nullptr) {
      return false;
    }
 
    uint32_t newPeriod = m_audioSource->getFormat().timerPeriodUs();
    if (newPeriod != m_timer_period_us && newPeriod > 0) {
      LOGI("Timer period changed from %u us to %u us",
           (unsigned)m_timer_period_us, (unsigned)newPeriod);
      m_timer_period_us = newPeriod;
      return true;
    }
    return false;
  }
 
  static void timerCallback(void *audioStreamerObj) {
    LOGD("timerCallback");
    if (audioStreamerObj == nullptr) {
      LOGE("audioStreamerObj is null");
      return;
    };
    RTSPAudioStreamerBase<Platform> *streamer =
        (RTSPAudioStreamerBase<Platform> *)audioStreamerObj;
    unsigned long start, stop;
 
    start = micros();
 
    int bytes = streamer->sendRtpPacketDirect();
 
    if (bytes < 0) {
      LOGW("Direct sending of RTP stream failed");
    } else if (bytes > 0) {
      uint32_t inc = streamer->computeTimestampIncrement(bytes);
      streamer->m_Timestamp += inc;
      LOGD("%i samples (ts inc) sent; timestamp: %u", inc,
           (unsigned)streamer->m_Timestamp);
    }
 
    stop = micros();
    if (stop - start > streamer->m_timer_period_us) {
      LOGW("RTP Stream can't keep up (took %lu us, %d is max)!", stop - start,
           streamer->m_timer_period_us);
    }
  }
 
 protected:
  const int STREAMING_BUFFER_SIZE = 1024 * 3;
  audio_tools::Vector<uint8_t> mRtpBuf;
 
  IAudioSource *m_audioSource = nullptr;
  int m_fragmentSize = 0;  // changed from samples to bytes !
  int m_timer_period_us = 20000;
  const int HEADER_SIZE = 12;  // size of the RTP header
  volatile bool m_timer_restart_needed =
      false;  // Flag for dynamic timer restart
 
  typename Platform::UdpSocketType
      *m_RtpSocket;  // RTP socket for streaming RTP packets to client
  typename Platform::UdpSocketType
      *m_RtcpSocket;  // RTCP socket for sending/receiving RTCP packages
 
  uint16_t m_RtpServerPort;   // RTP sender port on server
  uint16_t m_RtcpServerPort;  // RTCP sender port on server
 
  u_short m_SequenceNumber;
  uint32_t m_Timestamp;
  int m_SendIdx;
 
  IPAddress m_ClientIP;
  uint16_t m_ClientPort;
  uint32_t m_prevMsec;
 
  int m_udpRefCount;
 
  // TCP interleaved transport
  bool m_useTcpInterleaved;
  typename Platform::TcpClientType *m_RtspTcpSocket;
  int m_TcpRtpChannel;
  int m_TcpRtcpChannel;
 
  int m_payloadType = 96;
  int m_lastSamplesSent = 0;
  uint32_t m_Ssrc = 0x13F97E67;  // default fixed SSRC
  // MP3 packetization carry buffer to ensure whole-frame packets
  audio_tools::Vector<uint8_t> mMp3Carry;
  int mMp3CarryLen = 0;
 
  inline uint32_t computeTimestampIncrement(int bytesSent) {
    // Prefer exact samples sent (set by sender) if available
    int samples = 0;
    if (m_lastSamplesSent > 0) {
      samples = m_lastSamplesSent;
    } else if (m_audioSource) {
      samples = m_audioSource->getFormat().timestampIncrement();
    } else {
      samples = bytesSent / 2;  // crude fallback
    }
    return (uint32_t)samples;
  }
 
  inline void buildRtpHeader() {
    mRtpBuf[0] = 0x80;  // V=2
    mRtpBuf[1] = (uint8_t)(m_payloadType & 0x7F);
    if (m_payloadType == 14) {
      // Set Marker bit on each complete MP3 frame packet
      mRtpBuf[1] |= 0x80;
    }
    mRtpBuf[2] = (uint8_t)((m_SequenceNumber >> 8) & 0xFF);
    mRtpBuf[3] = (uint8_t)(m_SequenceNumber & 0xFF);
    mRtpBuf[4] = (uint8_t)((m_Timestamp >> 24) & 0xFF);
    mRtpBuf[5] = (uint8_t)((m_Timestamp >> 16) & 0xFF);
    mRtpBuf[6] = (uint8_t)((m_Timestamp >> 8) & 0xFF);
    mRtpBuf[7] = (uint8_t)(m_Timestamp & 0xFF);
    // SSRC
    mRtpBuf[8] = (uint8_t)((m_Ssrc >> 24) & 0xFF);
    mRtpBuf[9] = (uint8_t)((m_Ssrc >> 16) & 0xFF);
    mRtpBuf[10] = (uint8_t)((m_Ssrc >> 8) & 0xFF);
    mRtpBuf[11] = (uint8_t)(m_Ssrc & 0xFF);
  }
 
  inline void sendOut(uint16_t totalLen) {
    if (m_useTcpInterleaved && m_RtspTcpSocket != Platform::NULL_TCP_SOCKET) {
      LOGD("Sending TCP: %d", totalLen);
      uint8_t hdr[4];
      hdr[0] = 0x24;  // '$'
      hdr[1] = (uint8_t)m_TcpRtpChannel;
      hdr[2] = (uint8_t)((totalLen >> 8) & 0xFF);
      hdr[3] = (uint8_t)(totalLen & 0xFF);
      Platform::sendSocket(m_RtspTcpSocket, hdr, sizeof(hdr));
      Platform::sendSocket(m_RtspTcpSocket, mRtpBuf.data(), totalLen);
    } else {
      // If client IP is still unknown, attempt to learn it just-in-time
      tryLearnClientFromUdp(false);
      LOGI("Sending UDP: %d bytes (to %s:%d)", totalLen,
           m_ClientIP.toString().c_str(), m_ClientPort);
      Platform::sendUdpSocket(m_RtpSocket, mRtpBuf.data(), totalLen, m_ClientIP,
                              m_ClientPort);
    }
  }
 
  inline void tryLearnClientFromUdp(bool warnIfNone) {
    if (m_ClientIP == IPAddress(0, 0, 0, 0) && m_RtpSocket) {
      int avail = m_RtpSocket->parsePacket();
      if (avail > 0) {
        IPAddress learnedIp = m_RtpSocket->remoteIP();
        uint16_t learnedPort = m_RtpSocket->remotePort();
        if (learnedIp != IPAddress(0, 0, 0, 0)) {
          m_ClientIP = learnedIp;
          if (m_ClientPort == 0) m_ClientPort = learnedPort;
          LOGI("RTP learned client via UDP: %s:%u",
               m_ClientIP.toString().c_str(), (unsigned)m_ClientPort);
        }
      } else if (warnIfNone) {
        LOGW("Client IP unknown (0.0.0.0) and no inbound UDP yet");
      }
    }
  }
};
 
template <typename Platform>
class RTSPAudioStreamer : public RTSPAudioStreamerBase<Platform> {
 public:
  RTSPAudioStreamer() : RTSPAudioStreamerBase<Platform>() {
    LOGD("Creating RTSP Audio streamer with timer");
 
    // Setup timer callback for RTP streaming
    rtpTimer.setCallbackParameter(this);
 
    // CRITICAL FIX: Force timer to run in task context, not ISR context
    // ISR context (ESP_TIMER_ISR) has severe limitations that cause
    // LoadProhibited crashes Task context (ESP_TIMER_TASK) allows full object
    // access and FreeRTOS calls
    rtpTimer.setIsSave(
        true);  // true = use TimerCallbackInThread (ESP_TIMER_TASK)
    LOGI("RTSPAudioStreamer: Timer set to safe task mode (ESP_TIMER_TASK)");
  }
 
  RTSPAudioStreamer(IAudioSource &source) : RTSPAudioStreamer() {
    this->setAudioSource(&source);
  }
 
  void start() override {
    LOGI("Starting RTP Stream with timer");
 
    // Call base class start to initialize audio source and buffer
    RTSPAudioStreamerBase<Platform>::start();
 
    if (this->m_audioSource != nullptr) {
      // Start timer with period in microseconds using specialized callback
      if (!rtpTimer.begin(RTSPAudioStreamerBase<Platform>::timerCallback,
                          this->m_timer_period_us, audio_tools::US)) {
        LOGE("Could not start timer");
      }
      LOGI("timer: %u us", (unsigned)this->m_timer_period_us);
#ifdef ESP32
      LOGI("Free heap size: %i KB", esp_get_free_heap_size() / 1000);
#endif
    }
  }
 
  void updateTimer() {
    if (this->checkTimerPeriodChange()) {
      LOGI("Updating timer period to %u us", (unsigned)this->m_timer_period_us);
      rtpTimer.begin(this->m_timer_period_us, audio_tools::US);
    }
  }
 
  void stop() override {
    LOGI("Stopping RTP Stream with timer");
 
    // Stop timer first to prevent callbacks during cleanup
    rtpTimer.end();
 
    // Add small delay to ensure any running callbacks complete
    delay(50);
 
    // Call base class stop to stop audio source
    RTSPAudioStreamerBase<Platform>::stop();
 
    LOGI("RTP Stream stopped - ready for restart");
  }
 
 protected:
  audio_tools::TimerAlarmRepeating rtpTimer;
};
 
template <typename Platform>
class RTSPAudioStreamerUsingTask : public RTSPAudioStreamerBase<Platform> {
 public:
  RTSPAudioStreamerUsingTask(bool throttled = true)
      : RTSPAudioStreamerBase<Platform>() {
    LOGD("Creating RTSP Audio streamer with task");
    m_taskRunning = false;
    m_throttled = throttled;
  }
 
  RTSPAudioStreamerUsingTask(IAudioSource &source, bool throttled = true)
      : RTSPAudioStreamerUsingTask(throttled) {
    this->setAudioSource(&source);
  }
 
  virtual ~RTSPAudioStreamerUsingTask() { stop(); }
 
  void setTaskParameters(uint32_t stackSize, uint8_t priority, int core = -1) {
    if (!m_taskRunning) {
      m_taskStackSize = stackSize;
      m_taskPriority = priority;
      m_taskCore = core;
      LOGI("Task parameters set: stack=%d bytes, priority=%d, core=%d",
           stackSize, priority, core);
    } else {
      LOGW("Cannot change task parameters while streaming is active");
    }
  }
 
  void start() override {
    LOGI("Starting RTP Stream with task");
 
    // Call base class start to initialize audio source and buffer
    RTSPAudioStreamerBase<Platform>::start();
 
    if (this->m_audioSource != nullptr && !m_taskRunning) {
      m_taskRunning = true;
 
      // Create AudioTools Task for streaming
      if (!m_streamingTask.create("RTSPStreaming", m_taskStackSize,
                                  m_taskPriority, m_taskCore)) {
        LOGE("Failed to create streaming task");
        m_taskRunning = false;
        return;
      }
 
      // Set reference to this instance for the task
      m_streamingTask.setReference(this);
 
      // Start the task with our streaming loop
      // Reset throttle window timing
      m_send_counter = 0;
      m_last_throttle_us = micros();
 
      if (m_streamingTask.begin([this]() { this->streamingTaskLoop(); })) {
        LOGI("Streaming task started successfully");
        LOGI("Task: stack=%d bytes, priority=%d, core=%d, period=%d us",
             m_taskStackSize, m_taskPriority, m_taskCore,
             this->m_timer_period_us);
#ifdef ESP32
        LOGI("Free heap size: %i KB", esp_get_free_heap_size() / 1000);
#endif
      } else {
        LOGE("Failed to start streaming task");
        m_taskRunning = false;
      }
    }
  }
 
  void stop() override {
    LOGI("Stopping RTP Stream with task");
 
    if (m_taskRunning) {
      // Signal task to stop
      m_taskRunning = false;
 
      // Stop the AudioTools Task
      m_streamingTask.end();
 
      // Small delay to ensure clean shutdown
      delay(50);
    }
 
    // Call base class stop to stop audio source
    RTSPAudioStreamerBase<Platform>::stop();
 
    LOGI("RTP Stream with task stopped - ready for restart");
  }
 
  bool isTaskRunning() const { return m_taskRunning; }
 
  void setThrottled(bool isThrottled) { m_throttled = isThrottled; }
 
  void setFixedDelayMs(uint32_t delayUs) {
    this->m_fixed_delay_ms = delayUs;
    m_throttled = false;
  }
 
  void setThrottleInterval(uint32_t interval) {
    m_throttle_interval = interval;
  }
 
 protected:
  audio_tools::Task m_streamingTask;  
  volatile bool m_taskRunning;        
  uint32_t m_taskStackSize = 8192;  
  uint8_t m_taskPriority = 5;       
  int m_taskCore = -1;              
  bool m_throttled = true;          
  uint16_t m_fixed_delay_ms = 1;    
  uint32_t m_throttle_interval =
      1000;  
  uint32_t m_send_counter =
      0;  
  unsigned long m_last_throttle_us =
      0;  
 
  void streamingTaskLoop() {
    LOGD("Streaming task loop iteration");
 
    auto iterationStartUs = micros();
 
    // Always call the timer callback to send RTP packet
    RTSPAudioStreamerBase<Platform>::timerCallback(this);
 
    // Apply throttling (fixed delay per send + periodic correction)
    applyThrottling(iterationStartUs);
  }
 
  inline void applyThrottling(unsigned long iterationStartUs) {
    // Increment send counter and apply fixed delay after each send
    ++m_send_counter;
    delay(m_fixed_delay_ms);
 
    // If throttling by interval is enabled (m_throttled true), apply precise
    // correction periodically
    if (m_throttled && m_throttle_interval > 0) {
      // On timer period change, reset the throttle window to avoid drift
      if (this->checkTimerPeriodChange()) {
        LOGI("Timer period updated; resetting throttle window to %u us",
             (unsigned)this->m_timer_period_us);
        m_send_counter = 0;
        m_last_throttle_us = iterationStartUs;
        return;
      }
 
      if (m_send_counter >= m_throttle_interval) {
        // Expected elapsed time for N sends at configured period
        uint64_t expectedUs =
            (uint64_t)m_throttle_interval * (uint64_t)this->getTimerPeriodUs();
        unsigned long nowUs = micros();
        uint64_t actualUs = (uint64_t)(nowUs - m_last_throttle_us);
 
        if (actualUs < expectedUs) {
          uint32_t remainingUs = (uint32_t)(expectedUs - actualUs);
          // Sleep in ms then the remainder in us
          if (remainingUs >= 1000) {
            delay(remainingUs / 1000);
          }
          uint32_t remUs = remainingUs % 1000;
          if (remUs > 0) {
            delayMicroseconds(remUs);
          }
        } else if (actualUs > expectedUs + 1000) {
          LOGW("Throttling behind by %llu us over %u sends",
               (unsigned long long)(actualUs - expectedUs),
               (unsigned)m_throttle_interval);
        }
 
        // Reset window
        m_send_counter = 0;
        m_last_throttle_us = micros();
      }
    }
  }
};
 
}  // namespace audio_tools