RTP.cpp

#include "RTP.h"
 
#include "Context.h"
#include "Resources/TextureManager.h"
#include "Renderer/RenderTexture.h"
 
#include "Foundation/Logging/Logger.h"
#include "Foundation/Platform/Timer.h"
 
#undef CurrentTime
#include "jrtp/rtpsession.h"
#include "jrtp/rtpsessionparams.h"
#include "jrtp/rtpudpv4transmitter.h"
#include "jrtp/rtptcptransmitter.h"
#include "jrtp/rtppacket.h"
#include "jrtp/rtpsourcedata.h"
#include "jrtp/rtptcpaddress.h"
 
#include <assert.h>
#include <cinttypes>
 
#define INITIAL_BUFFER 64*3*5*7
 
using namespace Cogs::Core;
using namespace Cogs::Network;
 
namespace {
  constexpr const uint8_t naluStartCode[] = { 0x00, 0x00, 0x00, 0x01 };
 
  class CogsRTPMemoryManager : public jrtplib::RTPMemoryManager {
  public:
    static CogsRTPMemoryManager* instance() {
      static CogsRTPMemoryManager i;
 
      return &i;
    }
 
    void* AllocateBuffer(size_t numbytes, int /*memtype*/) {
      void* mem = malloc(numbytes);
 
      if (mem) {
        OsoMP_Allocate(COGSMEM_LibRTP, mem, numbytes, nullptr, 0);
      }
      return mem;
    }
 
    void FreeBuffer(void* ptr) {
      if (ptr) {
        OsoMP_Free(COGSMEM_LibRTP, ptr, nullptr, 0);
      }
      free(ptr);
    }
  };
 
int64_t timestamp_diff(uint32_t t1, uint32_t t0)
{
  // Computing timestamp difference:
  // We have timestamp a=t0 and b=t1 and timeline (4 bit example):
  // ---- ---- ---- ----
 
  // Normal Difference:
  // ---- -ab- ---- ----
  // diff = b-a = 6-5 = 1
 
  // Normal Negative Difference:
  // ---- -ba- ---- ----
  // diff = b-a = 6-5 = -1
 
  // Wraparound:
  // b--- ---- ---- ---a
  // diff = (b-a)+2^bits = (0-15)+16 = 1
 
  // Negative Wraparound:
  // a--- ---- ---- ---b
  // diff = (b-a)-2^bits = (15-0)-16 = -1
 
  // Deciding wraparound
  // abs(diff) >= 2^(bits-1) -> abs(diff) >= 2^3 = 8
 
  // int64_t diff = (int64_t)t1 - (int64_t)t0;
  // if(llabs(diff) > 0x7fffffffLL){
  //   if(diff > 0)
  //     diff -= (0xffffffffLL+1LL);
  //   else
  //     diff += (0xffffffffLL+1LL);
  // }
 
  int32_t diff = (int64_t)t1 - (int64_t)t0;
  return diff;
}
 
void timestamp_diff_test()
{
  // Normal
  assert(timestamp_diff(0, 0) == 0);
  assert(timestamp_diff(0xffffffffu, 0xffffffffu) == 0);
  assert(timestamp_diff(1, 1) == 0);
  assert(timestamp_diff(2, 1) == 1);
  assert(timestamp_diff(1, 2) == -1);
 
  // Wraparound
  assert(timestamp_diff(0, 0xffffffffu) == 1);
  assert(timestamp_diff(1, 0xffffffffu) == 2);
 
  // Negative Wraparound
  assert(timestamp_diff(0xffffffffu, 0) == -1);
  assert(timestamp_diff(0xffffffffu, 1) == -2);
}
} // namespace ...
 
static Cogs::Logging::Log logger = Cogs::Logging::getLogger("RTP");
 
Cogs::RTPStream::RTPStream(Cogs::Core::Context * context, uint32_t clock_rate, uint16_t port, Cogs::Network::AddrIn mc_adapter, bool udp):
  context(context),
  useUDP(udp),
  clock_rate(clock_rate),
  client_port(port)
{
  timestamp_diff_test();
  LOG_INFO(logger, "jrtplib Version: %s", jrtplib::RTPLibraryVersion::GetVersion().GetVersionString().c_str());
 
  tcpConnection.setDoNotProcess();
 
  session = new jrtplib::RTPSession(nullptr, CogsRTPMemoryManager::instance());
 
  jrtplib::RTPSessionParams sessionparams;
  sessionparams.SetOwnTimestampUnit(1.0/clock_rate); // Local timestamp unit (MUST BE SET)
  // sessionparams.SetAcceptOwnPackets(true);
  assert(!sessionparams.IsUsingPollThread());
 
  if (useUDP) {
    jrtplib::RTPUDPv4TransmissionParams transparams;
 
    transparams.SetPortbase(port);
    transparams.SetRTPReceiveBuffer(1024*1024);
    transparams.SetRTCPReceiveBuffer(1500*64);
    if (!(mc_adapter[0] == 0 && mc_adapter[1] == 0 && mc_adapter[2] == 0 && mc_adapter[3] == 0)){
      LOG_INFO(logger, "Multicast adapter: %s", mc_adapter.string().c_str());
      uint8_t ip_arr[4] = {mc_adapter[0], mc_adapter[1], mc_adapter[2], mc_adapter[3]};
      jrtplib::RTPIPv4Address rtp_ip;
      rtp_ip.SetIP(ip_arr);
      transparams.SetMulticastInterfaceIP(rtp_ip.GetIP());
    }
 
    int status = session->Create(sessionparams, &transparams, transparams.GetTransmissionProtocol());
    if (status < 0) {
      LOG_ERROR(logger, "session->Create Failed: %s", jrtplib::RTPGetErrorString(status).c_str());
      return;
    }
    jrtplib::RTPUDPv4TransmissionInfo *info = reinterpret_cast<jrtplib::RTPUDPv4TransmissionInfo*>(session->GetTransmissionInfo());
    rtp_port = info->GetRTPPort();
    rtcp_port = info->GetRTCPPort();
  }
  else {
    jrtplib::RTPTCPTransmissionParams transparams;
    int                               status = session->Create(sessionparams, &transparams, transparams.GetTransmissionProtocol());
 
    if (status < 0) {
      LOG_ERROR(logger, "session->Create Failed: %s", jrtplib::RTPGetErrorString(status).c_str());
      return;
    }
    rtp_port = port;
    rtcp_port = port + 1;
  }
 
  // session.SetDefaultPayloadType(96);
  // session.SetDefaultMark(false);
  // session.SetDefaultTimestampIncrement(160);
}
 
Cogs::RTPStream::~RTPStream()
{
  LOG_INFO(logger, "Shutdown");
  jrtplib::RTPTime delay(10.0);
  session->BYEDestroy(delay, "BYE", 9);
  for(auto &tmp : dest) {
    if(tmp.second.fp) {
      fclose(tmp.second.fp);
    }
    delete [] tmp.second.buffer;
  }
  delete session;
}
 
void Cogs::RTPStream::AddDestination(const uint8_t in_ip[4], uint16_t in_server_port)
{
  for (uint32_t i = 0; i < 4; i++) {
    ip[i] = in_ip[i];
  }
  server_port = in_server_port;
 
  LOG_INFO(logger, "Add destination %d.%d.%d.%d:%d-%d", ip[0], ip[1], ip[2], ip[3], server_port, server_port+1);
 
  int status = 0;
 
  if (useUDP) {
    status = session->AddDestination(jrtplib::RTPIPv4Address(ip, server_port));
  }
  else {
    Cogs::Network::SockaddrIn addr(Cogs::Network::AddrIn(ip), server_port);
 
    if (tcpConnection.connect(addr)) {
      LOG_INFO(logger, "RTPStream::AddDestination: Connected to %d.%d.%d.%d:%d-%d", ip[0], ip[1], ip[2], ip[3], server_port, server_port+1);
 
      tcpConnection.enableAutoReconnect();
      status = session->AddDestination(jrtplib::RTPTCPAddress(tcpConnection.getSocket()));
    }
    else {
      LOG_ERROR(logger, "RTPStream::AddDestination failed to connect to : %d.%d.%d.%d:%d", ip[0], ip[1], ip[2], ip[3], server_port);
    }
  }
  if(status < 0){
    LOG_ERROR(logger, "RTPStream::AddDestination failed: %s", jrtplib::RTPGetErrorString(status).c_str());
  }
}
 
void Cogs::RTPStream::JoinMulticastGroup(const uint8_t in_ip[4], uint16_t in_server_port)
{
  server_port = in_server_port;
  for(uint32_t i=0; i<4; i++) ip[i] = in_ip[i];
  LOG_INFO(logger, "Join multicast group %d.%d.%d.%d:%d-%d", ip[0], ip[1], ip[2], ip[3], server_port, server_port+1);
  jrtplib::RTPIPv4Address addr(ip, server_port);
  if(!session->SupportsMulticasting()){
    LOG_ERROR(logger, "session->SupportsMulticasting failed. (Possibly duplicate connection)");
    return;
  }
  int status = session->JoinMulticastGroup(addr);
  if(status < 0){
    LOG_ERROR(logger, "session->JoinMulticastGroup failed: %s",
              jrtplib::RTPGetErrorString(status).c_str());
  }
}
 
void Cogs::RTPStream::SendPacket(const uint8_t *buffer, size_t size)
{
  int status = session->SendPacket(buffer, size);
  if(status < 0){
    LOG_ERROR(logger, "session->SendPacket failed: %s",
              jrtplib::RTPGetErrorString(status).c_str());
    return;
  }
}
 
static bool g_first = true;
 
void Cogs::RTPStream::AddSSRC(uint32_t ssrc, Cogs::Core::Codec codec_in, ResourceId textureId)
{
  Dest d = {};
  d.first = g_first;
  g_first = false;
  d.textureHandle = Cogs::Core::TextureHandle(context->textureManager->getHandle(textureId));
  d.codec = codec_in;
  // if(false){
  //   std::string file = "test_stream_" + std::to_string(stream_number++) + ".h264";
  //   LOG_INFO(logger, "Writing file %s", file.c_str());
  //   d.fp = fopen(file.c_str(), "wb");
  // }
  d.buffer_size = INITIAL_BUFFER;
  d.buffer = new char[d.buffer_size];
 
  LOG_INFO(logger, "Adding SSRC 0x%x to RTPStream %p", ssrc, this);
 
  dest[ssrc] = std::move(d);
}
 
void Cogs::RTPStream::WriteData(Dest &dest_w)
{
  // Export to file...
  if(dest_w.fp) {
    fwrite(dest_w.nalu.data(), 1, dest_w.nalu.size(), dest_w.fp);
  }
 
  // Upload to decoder...
  if(HandleIsValid(dest_w.textureHandle)){
    IVideoDecoderContext* decoderContext = context->videoDecoderContext;
 
    if (decoderContext) {
      VideoDecoderPayload payload = {};
 
      payload.size = dest_w.nalu.size();
      payload.data = dest_w.nalu.data();
      payload.timestamp = dest_w.timestamp;
      payload.use_timestamp = true;
  
      decoderContext->streamVideoData(dest_w.textureHandle, payload);
    }
  }
  dest_w.nalu.clear();
}
 
Cogs::RTPStream::Dest *Cogs::RTPStream::GetLatestStream()
{
  uint64_t t = 0;
  Dest *ret = nullptr;
  for(auto &tmp : dest){
    if(tmp.second.receive_time >= t){
      t = tmp.second.receive_time;
      ret = &tmp.second;
    }
  }
  return ret;
}
 
void Cogs::RTPStream::Update()
{
  if(use_pool_thread) {
    session->BeginDataAccess();
  }
  else {
    session->Poll();
  }
 
  // Update TimestampUnit and handle BYE
  if(session->GotoFirstSource()){
    do{
      jrtplib::RTPSourceData *source = session->GetCurrentSourceInfo();
      source->SetTimestampUnit(1.0/clock_rate);
      if(source->ReceivedBYE()){
        uint32_t ssrc = source->GetSSRC();
        auto iter = dest.find(ssrc);
        if(iter != dest.end()){
          size_t bye_len;
          uint8_t *bye_reason = source->GetBYEReason(&bye_len);
          jrtplib::RTPTime rtp_time = source->GetBYETime();
          uint64_t bye_time = rtp_time.GetSeconds() * 1000000LL + rtp_time.GetMicroSeconds();
          LOG_INFO(logger, "Removing SSRC 0x%x from RTPStream %p. BYE: %s (t=%" PRIu64 ")",
                   ssrc, this, bye_reason ? std::string((char*)bye_reason, bye_len).c_str() : "", bye_time);
          if(iter->second.fp)
            fclose(iter->second.fp);
          delete [] iter->second.buffer;
          dest.erase(iter);
        }
      }
    } while(session->GotoNextSource());
  }
  else {
    LOG_ERROR(logger, "GotoFirstSource() failed for RTPStream %p.", this);
  }
 
  // Handle Incoming Data
  if(session->GotoFirstSourceWithData()){
    std::unique_lock lock(dest_mu);
    do {
      jrtplib::RTPPacket *packet;
 
      while ((packet = session->GetNextPacket()) != 0) {
        uint32_t ssrc = packet->GetSSRC();
        uint32_t eseq = packet->GetExtendedSequenceNumber();
        uint8_t* data = packet->GetPayloadData();
        size_t size = packet->GetPayloadLength();
        jrtplib::RTPTime rtime = packet->GetReceiveTime();
        uint64_t pkt_rtime = rtime.GetSeconds() * 1000000LL + rtime.GetMicroSeconds();
        uint64_t pkt_time = 0xF000000000000000ULL;
 
        if (packet->HasExtension()) {
          uint16_t ext_id = packet->GetExtensionID();
          size_t ext_size = packet->GetExtensionLength();
          uint8_t *ext_data = packet->GetExtensionData();
          // LOG_INFO(logger, "RTP Has extension id %u size %zu", ext_id, ext_size);
 
          // One byte header extension rfc5285
          if(rtp_one_byte_header_extension){
            // LOG_INFO(logger, "RTP one byte header extension");
            size_t idx = 0;
            while(idx < ext_size) {
              uint8_t ext_ext_header = ext_data[idx++];
              if(ext_ext_header == 0) continue; // Padding byte
              if(idx == ext_size) break;
              uint16_t ext_ext_id = (ext_ext_header&0xf0)>>4;
              size_t ext_ext_size = 1+ext_ext_header&0xf;
              if(ext_ext_id == 15) break;
              uint8_t *ext_ext_data = &ext_data[idx];
              // Synchronisation metadata: 64-bit timestamp format
              if(rfc6051_ext_id != 0 && ext_ext_id == rfc6051_ext_id){
                uint64_t pkt_time_ext = 0;
                uint64_t ntp =
                  ((uint64_t)ext_ext_data[7]<<0) |
                  ((uint64_t)ext_ext_data[6]<<8) |
                  ((uint64_t)ext_ext_data[5]<<16) |
                  ((uint64_t)ext_ext_data[4]<<24) |
                  ((uint64_t)ext_ext_data[3]<<32) |
                  ((uint64_t)ext_ext_data[2]<<40) |
                  ((uint64_t)ext_ext_data[1]<<48) |
                  ((uint64_t)ext_ext_data[0]<<56);
                auto tconv = jrtplib::RTPTime(jrtplib::RTPNTPTime((ntp>>32)&0xffffffffu, ntp&0xffffffffu));
                pkt_time_ext = tconv.GetSeconds() * 1000000LL + tconv.GetMicroSeconds();
                static uint32_t testvar = 0;
                if(testvar < 20){
                  LOG_INFO(logger, "NTP ts %" PRIu64 " ", ntp);
                  LOG_INFO(logger, "Using RTP 64 bit timestamp extension. (ext ts %" PRIu64 " old ts %" PRIu64 ")",
                           pkt_time_ext, pkt_time);
                  testvar++;
                }
                pkt_time = pkt_time_ext;
              }
              else{
                LOG_WARNING(logger, "RTP has unused OBH extension id %u size %zu", ext_ext_id, ext_ext_size);
              }
              idx += ext_ext_size;
            }
          }
 
          // Two byte header extension rfc5285
          else if(rtp_two_byte_header_extension){
            LOG_WARNING(logger, "RTP two byte header extension not supported."); // TODO add support for this?
          }
 
          // Unused extension
          else{
            LOG_WARNING(logger, "RTP has unused extension id %u size %zu.", ext_id, ext_size);
          }
        }
        if (pkt_time == 0xF000000000000000ULL) {
          jrtplib::RTPSourceData* source_data = session->GetCurrentSourceInfo();
 
          if (source_data->SR_HasInfo()) {
            uint32_t  ref_ts = source_data->SR_GetRTPTimestamp();
            double    ts_unit = source_data->GetTimestampUnit();
            uint32_t  pkt_ts = packet->GetTimestamp();
            int64_t   ts_diff = timestamp_diff(pkt_ts, ref_ts);
            int64_t   ts_diff_us = (int64_t)((double)ts_diff * (ts_unit * 1000000.0));
 
            // Flush packets if older then 5 seconds from senders report
            if(ts_diff_us >= -1000000.0*5.0){
              jrtplib::RTPTime  ref_ntp_rtp = jrtplib::RTPTime(source_data->SR_GetNTPTimestamp());
              uint64_t          ref_time = ref_ntp_rtp.GetSeconds() * 1000000LL + ref_ntp_rtp.GetMicroSeconds();
 
              pkt_time = ref_time + ts_diff_us;
            }
            else {
              source_data->FlushPackets();
              LOG_INFO(logger,"RTPStream %p: Flushed Packets SSRC 0x%x"
                        "(pkg age %" PRId64 " us pkt_ts %d ref_ts %d ts_unit %f)",
                        this, ssrc, ts_diff_us, pkt_ts, ref_ts, ts_unit);
            }
          }
          else {
            //LOG_ERROR(logger, "RTPStream %p has not received any SenderInfo.", this);
            // No sender report, just use packet timestamp
            pkt_time = packet->GetTimestamp();
          }
        }
 
        if (pkt_time != 0xF000000000000000ULL) {
          auto iter = dest.find(ssrc);
          if(iter == dest.end()){
            LOG_INFO(logger, "Adding SSRC 0x%x to RTPStream %p", ssrc, this);
            Dest d = {};
            d.first = g_first;
            g_first = false;
            d.textureHandle = Cogs::Core::TextureHandle::NoHandle;
            d.codec = codec;
            // if(false){
            //   std::string file = "test_stream_" + std::to_string(stream_number++) + ".h264";
            //   LOG_INFO(logger, "Writing file %s", file.c_str());
            //   d.fp = fopen(file.c_str(), "wb");
            // }
            d.buffer_size = INITIAL_BUFFER;
            d.buffer = new char[d.buffer_size];
            iter = dest.insert({ssrc, std::move(d)}).first;
          }
          Dest &d = iter->second;
 
          if(d.first){
            // uint32_t type = packet->GetPayloadType();
            // printf("Got packet extended seq %d SSRC %x PayloadType %d MarkerBit %s Extension %s\n",
            //        eseq, ssrc, type, packet->HasMarker()?"true":"false", packet->HasExtension()?"true":"false");
          }
          if (d.pps.size() && pkt_time != d.timestamp) {
            // int64_t ct = Cogs::Timer::currentTimeMicroseconds();
            // int64_t diff = ct - pkt_time;
            // int64_t rcv_diff = ct - pkt_rtime;
            // printf("diff %f\tts_diff_us %f\tts_diff %lld\trcv_diff %f\n", diff/1000000.0, ts_diff_us/1000000.0, ts_diff, rcv_diff/1000000.0);
          }
 
          d.prev_receive_time = d.receive_time;
          d.receive_time = pkt_rtime;
          d.prev_timestamp = d.timestamp;
          d.timestamp = pkt_time;
          d.size = size;
          d.prev_eseq = d.eseq;
          d.eseq = eseq;
 
          // for(size_t i=0; i<size && i<=1500; i++){
          //   printf("%2X ", data[i]);
          //   if(i%16 == 15) printf("\n");
          // }
 
          d.size_akk += (uint32_t)size;
          if(d.receive_time-d.rate_time > 1ull<<27){
            double rt_diff = (d.receive_time-d.rate_time)*(1.0/(1ull<<32));
            double rate = (d.size_akk*8)/rt_diff;
            double a = 1.0/8;
            d.rate = (1.0-a)*d.rate + a*rate;
            d.rate_time = d.receive_time;
            d.size_akk = 0;
          }
 
 
          if (d.codec == Cogs::Core::Codec::None) {
          }
          else if (d.codec == Cogs::Core::Codec::HEVC) {
            if((data[0] & 0x80) != 0){
              LOG_ERROR(logger, "Corrupt RTP HEVC packet (f != 0). (RTPStream %p). Possible MPEG-TS stream not supported.", this);
              session->DeletePacket(packet);
              continue;
            }
            uint32_t type = (data[0]>>1)&0x3f;
            // uint32_t layerId = (data[0]&0x1)<<8 | data[1]&0xf8;
            uint32_t TID = data[1]&0x7;
            if(TID == 0){
              // A TID value of 0 is illegal to ensure that there is at least one bit in the NAL unit
              // header equal to 1, so to enable independent considerations of start code emulations
              // in the NAL unit header and in the NAL unit payload data.
              LOG_ERROR(logger, "Corrupt RTP H265 packet (TID == 0).");
              session->DeletePacket(packet);
              continue;
            }
            // TODO DONL if(sprop-max-don-diff > 0)
 
            if (type <= 40) {
              processH265NALU(d, type, data, size);
            }
            else if (type == 48) { // Agregation Packets AP
              LOG_ERROR(logger, "H265 aggregation packets are not supported.");
              assert(false); // TODO
            }
            else if (type == 49) { // Fragment Unit FUs
              uint32_t s = (data[2] >> 7) & 0x1; // Start of fragmented NAL unit
              uint32_t e = (data[2] >> 6) & 0x1; // End of fragmented NAL unit
              uint32_t fu_type = data[2] & 0x3f;
 
              assert(fu_type >= 0 && fu_type <= 40);
 
              // TODO DONL if(sprop-max-don-diff > 0)
              if (s) {
                d.nalu.write(naluStartCode, sizeof(naluStartCode));
                d.nalu.write8(static_cast<uint8_t>(fu_type << 1));
                d.nalu.write8(1);
              }
              size_t beg = 3; // TODO 5 DONL
              size_t siz = size-beg;
              d.nalu.write(&data[beg], siz);
              if (e) {
                WriteData(d);
              }
            }
            else{
              LOG_ERROR(logger, "Unsupported H265 packet of type %d encountered.", type);
              //assert(false);
            }
          }
          else if (d.codec == Cogs::Core::Codec::H264) {
            if((data[0] & 0x80 ) != 0){
              LOG_ERROR(logger, "Corrupt RTP H264 packet (f != 0). (RTPStream %p). Possible MPEG-TS stream not supported.", this);
              session->DeletePacket(packet);
              continue;
            }
            uint32_t nri = (data[0] >> 5) & 0x3f;
            uint32_t rtp_type = data[0] & 0x1f;
 
            if (rtp_type <= 23){ // Single NAL Unit Packet (NAL unit)
              processH264NALU(d, rtp_type, data, size);
            }
            else if(rtp_type == 24) { // Single-time aggregation packet (STAP-A)
              uint8_t* read = data + 1;
 
              while (read < (data + size)) {
                uint16_t naluSize = ntohs(*reinterpret_cast<uint16_t*>(read));
                read += 2;
 
                if ((read[0] & 0x80) != 0) {
                  LOG_ERROR(logger, "Corrupt RTP H264 packet: Non-zero 'f' field in STAP-A NALU. (RTPStream %p). Possible MPEG-TS stream not supported.", this);
                }
                else {
                  processH264NALU(d, read[0] & 0x1f, read, naluSize);
                }
                read += naluSize;
              }
            }
            else if (rtp_type == 25) { // Single-time aggregation packet (STAP_B)
              LOG_ERROR(logger, "STAP-B packets are not supported.");
              assert(false);
            }
            else if ((rtp_type == 26) || (rtp_type == 27)) { // Multi-time aggregation packet (MTAP16, MTAP24)
              LOG_ERROR(logger, "MTAP packets are not supported.");
              assert(false);
            }
            else if ((rtp_type == 28) || (rtp_type == 29)) { // Fragmentation unit (FU-A, FU-B)
              assert(rtp_type == 28);
              uint32_t s = (data[1]>>7)&0x1; // Start of fragmented NAL unit
              uint32_t e = (data[1]>>6)&0x1; // End of fragmented NAL unit
              uint32_t r = (data[1]>>5)&0x1;
              assert(r == 0);
              uint32_t type = (data[1]>>0)&0x1f;
              if(rtp_type == 29){
                //uint16_t DON = (data[2]<<8) | (data[3]<<0);
              }
              if(type == 1){ // Coded slice of a non-IDR picture
                if(s){
                  d.nalu.write(naluStartCode, sizeof(naluStartCode));
                  d.nalu.write8(static_cast<uint8_t>((nri<<5) | type));
                }
                size_t beg = (rtp_type == 29) ? 4 : 2;
 
                d.nalu.write(&data[beg], size - beg);
                if (e) {
                  WriteData(d);
                }
              }
              else if(type == 5){ // Coded slice of an IDR picture
                if(s){
                  d.nalu.write(naluStartCode, sizeof(naluStartCode));
                  d.nalu.write8(static_cast<uint8_t>((nri<<5) | type));
                }
                size_t beg = (rtp_type == 29) ? 4 : 2;
 
                d.nalu.write(&data[beg], size - beg);
                if (e) {
                  WriteData(d);
                }
              }
              else if (type == 0) { // Unspecified type
                if (s) {
                  d.nalu.write(naluStartCode, sizeof(naluStartCode));
                  d.nalu.write8(static_cast<uint8_t>((nri << 5) | type));
                }
                size_t beg = (rtp_type == 29) ? 4 : 2;
 
                d.nalu.write(&data[beg], size - beg);
                if (e) {
                  WriteData(d);
                }
              }
              else{
                assert(false);
              }
            }
            else{
              assert(false);
            }
          }
          else{
            assert(false); // Unsupported codec
          }
        }
        session->DeletePacket(packet);
      }
    }
    while(session->GotoNextSourceWithData());
  }
  if (use_pool_thread) {
    session->EndDataAccess();
  }
}
 
void Cogs::RTPStream::processH264NALU(Dest& dest, uint32_t rtpType, const void* data, size_t size) {
  assert (rtpType <= 23);
 
  if ((rtpType == 7) && !dest.sps.size()) {
    dest.sps = std::string(static_cast<const char*>(data), size);
    LOG_INFO(logger, "RTP received H264 SPS (RTPStream %p)", this);
  }
  else if ((rtpType == 8) && !dest.pps.size()){
    dest.pps = std::string(static_cast<const char*>(data), size);
    LOG_INFO(logger, "RTP received H264 PPS (RTPStream %p)", this);
  }
  dest.nalu.write(naluStartCode, sizeof(naluStartCode));
  dest.nalu.write(data, size);
  WriteData(dest);
}
 
void Cogs::RTPStream::processH265NALU(Dest& dest, uint32_t rtpType, const void* data, size_t size) {
  assert (rtpType <= 40);
 
  if ((rtpType == 32) && !dest.vps.size()) {
    dest.vps = std::string(static_cast<const char*>(data), size);
    LOG_INFO(logger, "RTP receive H265 VPS (RTPStream %p)", this);
  }
  else if((rtpType == 33) && !dest.sps.size()) {
    dest.sps = std::string(static_cast<const char*>(data), size);
    LOG_INFO(logger, "RTP receive H265 SPS (RTPStream %p)", this);
  }
  else if((rtpType == 34) && !dest.pps.size()) {
    dest.pps = std::string(static_cast<const char*>(data), size);
    LOG_INFO(logger, "RTP receive H265 PPS (RTPStream %p)", this);
  }
  dest.nalu.write(naluStartCode, sizeof(naluStartCode));
  dest.nalu.write(data, size);
  WriteData(dest);
}