diff --git a/ambed/ccontroller.cpp b/ambed/ccontroller.cpp index 85dbcd3..2422654 100644 --- a/ambed/ccontroller.cpp +++ b/ambed/ccontroller.cpp @@ -128,6 +128,8 @@ void CController::Task(void) uint8 CodecOut1; uint8 CodecOut2; uint8 CodecOut3; + uint8 CodecsOut[STREAM_MAX_OUTPUTS]; + uint32 Transaction; uint16 StreamId; CStream *Stream; @@ -135,7 +137,48 @@ void CController::Task(void) if ( m_Socket.Receive(&Buffer, &Ip, 20) != -1 ) { // crack packet - if ( IsValidOpenstreamPacket(Buffer, &Callsign, &CodecIn, &CodecOut1, &CodecOut2, &CodecOut3) ) + if ( IsValidCapsPacket(Buffer) ) + { + EncodeCapsPacket(&Buffer); + m_Socket.Send(Buffer, Ip); + } + else if ( IsValidExtOpenstreamPacket(Buffer, &Callsign, &CodecIn, CodecsOut, &Transaction) ) + { + // an extended open always gets a reply, CodecIn is CODEC_NONE when invalid + Stream = NULL; + if ( CodecIn != CODEC_NONE ) + { + // retransmitted open: same peer, transaction and codecs get the same stream + Lock(); + for ( size_t i = 0; (i < m_Streams.size()) && (Stream == NULL); i++ ) + { + if ( m_Streams[i]->IsTransaction(Ip, Transaction, CodecIn, CodecsOut) ) + { + Stream = m_Streams[i]; + } + } + Unlock(); + if ( Stream == NULL ) + { + std::cout << "Extended stream Open from " << Callsign << " at " << Ip << " CodecIn=" << (int)CodecIn + << " Out=" << (int)CodecsOut[0] << "," << (int)CodecsOut[1] << "," << (int)CodecsOut[2] << "," << (int)CodecsOut[3] << std::endl; + Stream = OpenStream(Callsign, Ip, CodecIn, CodecsOut[0], CodecsOut[1], CodecsOut[2], CodecsOut[3], true, Transaction); + } + } + + // send back details + if ( Stream != NULL ) + { + EncodeStreamDescrPacket(&Buffer, *Stream); + } + else + { + EncodeNoStreamAvailablePacket(&Buffer); + } + Buffer.Append((uint32)htonl(Transaction)); + m_Socket.Send(Buffer, Ip); + } + else if ( IsValidOpenstreamPacket(Buffer, &Callsign, &CodecIn, &CodecOut1, &CodecOut2, &CodecOut3) ) { std::cout << "Stream Open from " << Callsign << " at " << Ip << " CodecIn=" << (int)CodecIn @@ -209,21 +252,32 @@ void CController::Task(void) //////////////////////////////////////////////////////////////////////////////////////// // streams management -CStream *CController::OpenStream(const CCallsign &Callsign, const CIp &Ip, uint8 CodecIn, uint8 CodecOut1, uint8 CodecOut2, uint8 CodecOut3) +CStream *CController::OpenStream(const CCallsign &Callsign, const CIp &Ip, uint8 CodecIn, uint8 CodecOut1, uint8 CodecOut2, uint8 CodecOut3, uint8 CodecOut4, bool Extended, uint32 Transaction) { CStream *stream = NULL; + // with no vocodec hardware, extended streams still get a few software only slots + uint16 uiMaxStreams = m_uiMaxStreams; + if ( (uiMaxStreams == 0) && Extended ) + { + uiMaxStreams = SOFTWARE_MAX_STREAMS; + } + // check capacity - if ( m_Streams.size() >= (size_t)m_uiMaxStreams ) + if ( m_Streams.size() >= (size_t)uiMaxStreams ) { - std::cout << "Max streams reached (" << m_uiMaxStreams << "), rejecting" << std::endl; + std::cout << "Max streams reached (" << uiMaxStreams << "), rejecting" << std::endl; return NULL; } // create a new stream m_uiLastStreamId = (m_uiLastStreamId + 1); - m_uiLastStreamId = (m_uiLastStreamId > m_uiMaxStreams) ? 1 : m_uiLastStreamId; - stream = new CStream(m_uiLastStreamId, Callsign, Ip, CodecIn, CodecOut1, CodecOut2, CodecOut3); + m_uiLastStreamId = (m_uiLastStreamId > uiMaxStreams) ? 1 : m_uiLastStreamId; + stream = new CStream(m_uiLastStreamId, Callsign, Ip, CodecIn, CodecOut1, CodecOut2, CodecOut3, CodecOut4); + if ( Extended ) + { + stream->SetTransaction(Transaction); + } if ( stream->Init(TRANSCODER_PORT+m_uiLastStreamId) ) { std::cout << "Opened stream " << m_uiLastStreamId << std::endl; @@ -295,6 +349,45 @@ void CController::CloseStream(uint16 StreamId) //////////////////////////////////////////////////////////////////////////////////////// // packet decoding helpers +bool CController::IsValidCapsPacket(const CBuffer &Buffer) +{ + uint8 tag[] = { 'A','M','B','E','D','C','A','P','S' }; + + return ( (Buffer.size() == 9) && (Buffer.Compare(tag, sizeof(tag)) == 0) ); +} + +bool CController::IsValidExtOpenstreamPacket(const CBuffer &Buffer, CCallsign *Callsign, uint8 *CodecIn, uint8 *CodecsOut, uint32 *Transaction) +{ + uint8 tag[] = { 'A','M','B','E','D','O','S' }; + + bool valid = false; + // Extended format: 24 bytes, callsign + codecin + 4 zero padded output codecs + BE32 transaction + if ( (Buffer.size() == 24) && (Buffer.Compare(tag, sizeof(tag)) == 0) ) + { + Callsign->SetCallsign(&(Buffer.data()[7]), 8); + *CodecIn = Buffer.data()[15]; + ::memcpy(CodecsOut, &(Buffer.data()[16]), STREAM_MAX_OUTPUTS); + ::memcpy(Transaction, &(Buffer.data()[20]), sizeof(uint32)); + *Transaction = ntohl(*Transaction); + + // outputs: at least one, no gaps, distinct and different from input + int n = 0; + while ( (n < STREAM_MAX_OUTPUTS) && (CodecsOut[n] != CODEC_NONE) ) n++; + bool ok = Callsign->IsValid() && IsValidExtCodec(*CodecIn) && (n > 0); + for ( int i = 0; (i < STREAM_MAX_OUTPUTS) && ok; i++ ) + { + ok = (i >= n) ? (CodecsOut[i] == CODEC_NONE) : + (IsValidExtCodec(CodecsOut[i]) && (CodecsOut[i] != *CodecIn) && (::memchr(CodecsOut, CodecsOut[i], i) == NULL)); + } + if ( !ok ) + { + *CodecIn = CODEC_NONE; + } + valid = true; + } + return valid; +} + bool CController::IsValidKeepAlivePacket(const CBuffer &Buffer, CCallsign *Callsign) { uint8 tag[] = { 'A','M','B','E','D','P','I','N','G' }; @@ -407,6 +500,23 @@ void CController::EncodeNoStreamAvailablePacket(CBuffer *Buffer) Buffer->Set(tag, sizeof(tag)); } +void CController::EncodeCapsPacket(CBuffer *Buffer) +{ + uint8 tag[] = { 'A','M','B','E','D','C','A','P','S' }; + + // codec mask, bit n = codec n + // Codec2, IMBE and PCM are software, vocodec channels always come in AMBE+ / AMBE2+ pairs + uint32 mask = (1 << CODEC_CODEC2) | (1 << CODEC_IMBE) | (1 << CODEC_PCM_S16LE_8K); + if ( g_Vocodecs.GetNbChannels() > 0 ) + { + mask |= (1 << CODEC_AMBEPLUS) | (1 << CODEC_AMBE2PLUS); + } + Buffer->Set(tag, sizeof(tag)); + Buffer->Append((uint8)CAPS_VERSION); + Buffer->Append((uint8)STREAM_MAX_OUTPUTS); + Buffer->Append((uint32)htonl(mask)); +} + //////////////////////////////////////////////////////////////////////////////////////// // codec helpers @@ -421,3 +531,8 @@ bool CController::IsValidCodecOut(uint8 codec) return ((codec == CODEC_AMBEPLUS) || (codec == CODEC_AMBE2PLUS) || (codec == CODEC_CODEC2) || (codec == CODEC_IMBE)); } +bool CController::IsValidExtCodec(uint8 codec) +{ + return (IsValidCodecIn(codec) || (codec == CODEC_PCM_S16LE_8K)); +} + diff --git a/ambed/ccontroller.h b/ambed/ccontroller.h index 2899794..a68f470 100644 --- a/ambed/ccontroller.h +++ b/ambed/ccontroller.h @@ -55,7 +55,7 @@ public: void SetListenIp(const CIp &ip) { m_Ip = ip; } // streams management - CStream *OpenStream(const CCallsign &, const CIp &, uint8, uint8, uint8, uint8); + CStream *OpenStream(const CCallsign &, const CIp &, uint8, uint8, uint8, uint8, uint8 = CODEC_NONE, bool = false, uint32 = 0); void CloseStream(CStream *); void CloseStream(uint16); @@ -68,15 +68,19 @@ protected: bool IsValidKeepAlivePacket(const CBuffer &, CCallsign *); bool IsValidOpenstreamPacket(const CBuffer &, CCallsign *, uint8 *, uint8 *, uint8 *, uint8 *); bool IsValidClosestreamPacket(const CBuffer &, uint16 *); + bool IsValidCapsPacket(const CBuffer &); + bool IsValidExtOpenstreamPacket(const CBuffer &, CCallsign *, uint8 *, uint8 *, uint32 *); // packet encoding helpers void EncodeKeepAlivePacket(CBuffer *); void EncodeStreamDescrPacket(CBuffer *, const CStream &); void EncodeNoStreamAvailablePacket(CBuffer *); + void EncodeCapsPacket(CBuffer *); // codec helpers bool IsValidCodecIn(uint8); bool IsValidCodecOut(uint8); + bool IsValidExtCodec(uint8); protected: // control socket diff --git a/ambed/cstream.cpp b/ambed/cstream.cpp index 83d8902..e71209e 100644 --- a/ambed/cstream.cpp +++ b/ambed/cstream.cpp @@ -35,6 +35,32 @@ //////////////////////////////////////////////////////////////////////////////////////// // define +// frame size of a codec on the wire +static int FrameSize(uint8 codec) +{ + switch ( codec ) + { + case CODEC_AMBEPLUS: + case CODEC_AMBE2PLUS: return AMBE_FRAME_SIZE; + case CODEC_CODEC2: return CODEC2_FRAME_SIZE; + case CODEC_IMBE: return IMBE_FRAME_SIZE; + case CODEC_PCM_S16LE_8K: return PCM_FRAME_SIZE; + default: return 0; + } +} + +// apply gain to 160 PCM samples, clipped to int16 range +static void ScalePcm(const int16_t *in, int16_t *out, float gain) +{ + for (int i = 0; i < 160; i++) + { + int32_t sample = (int32_t)(in[i] * gain); + if (sample > 32767) sample = 32767; + else if (sample < -32768) sample = -32768; + out[i] = (int16_t)sample; + } +} + //////////////////////////////////////////////////////////////////////////////////////// // constructor @@ -46,6 +72,9 @@ CStream::CStream() m_uiCodecOut1 = CODEC_NONE; m_uiCodecOut2 = CODEC_NONE; m_uiCodecOut3 = CODEC_NONE; + m_uiCodecOut4 = CODEC_NONE; + m_bExtended = false; + m_uiTransaction = 0; m_bStopThread = false; m_pThread = NULL; m_VocodecChannel = NULL; @@ -68,7 +97,7 @@ CStream::CStream() m_nCodec2QueueSize = 0; } -CStream::CStream(uint16 uiId, const CCallsign &Callsign, const CIp &Ip, uint8 uiCodecIn, uint8 uiCodecOut1, uint8 uiCodecOut2, uint8 uiCodecOut3) +CStream::CStream(uint16 uiId, const CCallsign &Callsign, const CIp &Ip, uint8 uiCodecIn, uint8 uiCodecOut1, uint8 uiCodecOut2, uint8 uiCodecOut3, uint8 uiCodecOut4) { m_uiId = uiId; m_Callsign = Callsign; @@ -78,6 +107,9 @@ CStream::CStream(uint16 uiId, const CCallsign &Callsign, const CIp &Ip, uint8 ui m_uiCodecOut1 = uiCodecOut1; m_uiCodecOut2 = uiCodecOut2; m_uiCodecOut3 = uiCodecOut3; + m_uiCodecOut4 = uiCodecOut4; + m_bExtended = false; + m_uiTransaction = 0; m_bStopThread = false; m_pThread = NULL; m_VocodecChannel = NULL; @@ -122,8 +154,58 @@ bool CStream::Init(uint16 uiPort) ok = m_Socket.Open(g_AmbeServer.GetListenIp(), uiPort); if ( ok ) { + // Extended Codec2, IMBE or PCM input: software decode, encoder channels only for requested AMBE outputs + if ( m_bExtended && (m_uiCodecIn != CODEC_AMBEPLUS) && (m_uiCodecIn != CODEC_AMBE2PLUS) ) + { + if ( HasCodecOut(CODEC_AMBEPLUS) ) + ok &= ((m_EncoderChannel1 = g_Vocodecs.OpenEncoderChannel(CODEC_AMBEPLUS)) != NULL); + if ( HasCodecOut(CODEC_AMBE2PLUS) ) + ok &= ((m_EncoderChannel2 = g_Vocodecs.OpenEncoderChannel(CODEC_AMBE2PLUS)) != NULL); + + if ( ok ) + { + // store port + m_uiPort = uiPort; + + // same gains as the legacy Codec2/IMBE input modes, PCM input is used as is + if ( m_uiCodecIn == CODEC_CODEC2 ) + { + m_pCodec2Decoder = new CCodec2(true); + m_fCodec2DecodeGain = pow(10.0f, CODEC2_DECODE_GAIN / 20.0f); + m_fImbeGain = pow(10.0f, CODEC2_TO_IMBE_GAIN / 20.0f); + m_pSignalProcessor1 = new CSignalProcessor((float)CODEC2_TO_AMBEPLUS_GAIN); + m_pSignalProcessor2 = new CSignalProcessor((float)CODEC2_TO_AMBE2PLUS_GAIN); + } + else if ( m_uiCodecIn == CODEC_IMBE ) + { + m_pImbeDecoder = new imbe_vocoder(); + m_fImbeGain = pow(10.0f, IMBE_DECODE_GAIN / 20.0f); + m_fCodec2Gain = pow(10.0f, IMBE_TO_CODEC2_GAIN / 20.0f); + m_pSignalProcessor1 = new CSignalProcessor((float)IMBE_TO_AMBEPLUS_GAIN); + m_pSignalProcessor2 = new CSignalProcessor((float)IMBE_TO_AMBE2PLUS_GAIN); + } + if ( HasCodecOut(CODEC_CODEC2) ) + m_pCodec2Encoder = new CCodec2(true); + if ( HasCodecOut(CODEC_IMBE) ) + m_pImbeEncoder = new imbe_vocoder(); + + // start thread + m_pThread = new std::thread(CStream::Thread, this); + + // init timeout system + m_LastActivity.Now(); + m_iTotalPackets = 0; + m_iLostPackets = 0; + } + else + { + std::cout << "Error opening stream : no suitable encoder channels available" << std::endl; + if (m_EncoderChannel1 != NULL) { m_EncoderChannel1->Close(); m_EncoderChannel1 = NULL; } + if (m_EncoderChannel2 != NULL) { m_EncoderChannel2->Close(); m_EncoderChannel2 = NULL; } + } + } // Codec2 input mode: need two encoder channels + software decoder + software IMBE encoder - if (m_uiCodecIn == CODEC_CODEC2) + else if (m_uiCodecIn == CODEC_CODEC2) { // Create Codec2 software decoder m_pCodec2Decoder = new CCodec2(true); // true = 3200bps mode @@ -244,7 +326,13 @@ bool CStream::Init(uint16 uiPort) else { // Normal AMBE input mode: open a VocodecChannel for hardware transcoding - ok &= ((m_VocodecChannel = g_Vocodecs.OpenChannel(m_uiCodecIn, m_uiCodecOut1)) != NULL); + // extended streams always pair with the other AMBE codec, dropped if not requested + uint8 uiCodecHw = m_uiCodecOut1; + if ( m_bExtended ) + { + uiCodecHw = (m_uiCodecIn == CODEC_AMBEPLUS) ? CODEC_AMBE2PLUS : CODEC_AMBEPLUS; + } + ok &= ((m_VocodecChannel = g_Vocodecs.OpenChannel(m_uiCodecIn, uiCodecHw)) != NULL); if ( ok ) { @@ -252,19 +340,25 @@ bool CStream::Init(uint16 uiPort) m_uiPort = uiPort; // Enable Codec2 encoding if second output is Codec2 - if (m_uiCodecOut2 == CODEC_CODEC2) + if (HasCodecOut(CODEC_CODEC2)) { m_VocodecChannel->EnableCodec2(true); std::cout << "Codec2 software encoding enabled for stream" << std::endl; } // Enable IMBE encoding if third output is IMBE - if (m_uiCodecOut3 == CODEC_IMBE) + if (HasCodecOut(CODEC_IMBE)) { m_VocodecChannel->EnableImbe(true); std::cout << "IMBE software encoding enabled for stream" << std::endl; } + // Enable PCM tap if PCM is requested + if (HasCodecOut(CODEC_PCM_S16LE_8K)) + { + m_VocodecChannel->EnablePcm(true); + } + // start thread m_pThread = new std::thread(CStream::Thread, this); @@ -300,11 +394,10 @@ void CStream::Close(void) if ( m_pThread == NULL ) return; - // For IMBE/Codec2 input modes, wait for encoder queues to drain + // For IMBE/Codec2/PCM input modes, wait for encoder queues to drain // This ensures packets still in DVSI hardware FIFOs get processed and sent back // The thread must keep running during drain to collect hardware responses - if ( (m_uiCodecIn == CODEC_IMBE || m_uiCodecIn == CODEC_CODEC2) && - m_EncoderChannel1 != NULL && m_EncoderChannel2 != NULL ) + if ( m_EncoderChannel1 != NULL || m_EncoderChannel2 != NULL ) { const int MAX_DRAIN_MS = 500; const int DRAIN_POLL_MS = 10; @@ -314,13 +407,17 @@ void CStream::Close(void) // Wait for voice queues to drain (PCM waiting to be sent to hardware) while ( waitedMs < MAX_DRAIN_MS ) { - CPacketQueue *v1 = m_EncoderChannel1->GetVoiceQueue(); - bool v1e = v1->empty(); - m_EncoderChannel1->ReleaseVoiceQueue(); - - CPacketQueue *v2 = m_EncoderChannel2->GetVoiceQueue(); - bool v2e = v2->empty(); - m_EncoderChannel2->ReleaseVoiceQueue(); + bool v1e = true, v2e = true; + if ( m_EncoderChannel1 != NULL ) + { + v1e = m_EncoderChannel1->GetVoiceQueue()->empty(); + m_EncoderChannel1->ReleaseVoiceQueue(); + } + if ( m_EncoderChannel2 != NULL ) + { + v2e = m_EncoderChannel2->GetVoiceQueue()->empty(); + m_EncoderChannel2->ReleaseVoiceQueue(); + } if ( v1e && v2e ) break; @@ -338,13 +435,17 @@ void CStream::Close(void) waitedMs = 0; while ( waitedMs < MAX_DRAIN_MS ) { - CPacketQueue *q1 = m_EncoderChannel1->GetPacketQueueOut(); - bool q1e = q1->empty(); - m_EncoderChannel1->ReleasePacketQueueOut(); - - CPacketQueue *q2 = m_EncoderChannel2->GetPacketQueueOut(); - bool q2e = q2->empty(); - m_EncoderChannel2->ReleasePacketQueueOut(); + bool q1e = true, q2e = true; + if ( m_EncoderChannel1 != NULL ) + { + q1e = m_EncoderChannel1->GetPacketQueueOut()->empty(); + m_EncoderChannel1->ReleasePacketQueueOut(); + } + if ( m_EncoderChannel2 != NULL ) + { + q2e = m_EncoderChannel2->GetPacketQueueOut()->empty(); + m_EncoderChannel2->ReleasePacketQueueOut(); + } bool swQueueEmpty; if (m_uiCodecIn == CODEC_CODEC2) @@ -453,6 +554,7 @@ void CStream::Close(void) { m_VocodecChannel->EnableCodec2(false); m_VocodecChannel->EnableImbe(false); + m_VocodecChannel->EnablePcm(false); m_VocodecChannel->Close(); m_VocodecChannel = NULL; } @@ -477,6 +579,7 @@ void CStream::Close(void) // Clear software queues and reset counters while ( !m_ImbeQueue.empty() ) m_ImbeQueue.pop(); while ( !m_Codec2Queue.empty() ) m_Codec2Queue.pop(); + while ( !m_ReplyQueue.empty() ) m_ReplyQueue.pop(); m_nImbeQueueSize = 0; m_nCodec2QueueSize = 0; @@ -513,7 +616,11 @@ void CStream::Thread(CStream *This) void CStream::Task(void) { // Route to appropriate task handler based on input codec - if (m_uiCodecIn == CODEC_CODEC2) + if (m_bExtended && (m_uiCodecIn != CODEC_AMBEPLUS) && (m_uiCodecIn != CODEC_AMBE2PLUS)) + { + TaskSoftwareInput(); + } + else if (m_uiCodecIn == CODEC_CODEC2) { TaskCodec2Input(); } @@ -528,8 +635,6 @@ void CStream::Task(void) CIp Ip; uint8 uiPid; uint8 Ambe[AMBE_FRAME_SIZE]; - uint8 Codec2[CODEC2_FRAME_SIZE]; - uint8 Imbe[IMBE_FRAME_SIZE]; CAmbePacket *packet; CPacketQueue *queue; @@ -554,21 +659,9 @@ void CStream::Task(void) // Check if we have a staged packet waiting for software codecs if ( m_pStagedPacket != NULL ) { - bool codec2Ready = (m_uiCodecOut2 != CODEC_CODEC2) || m_VocodecChannel->HasCodec2Data(); - bool imbeReady = (m_uiCodecOut3 != CODEC_IMBE) || m_VocodecChannel->HasImbeData(); - - if ( codec2Ready && imbeReady ) + if ( SendDvFramePacket(m_pStagedPacket) ) { - // software codecs caught up — get the data and send - ::memset(Codec2, 0, CODEC2_FRAME_SIZE); - ::memset(Imbe, 0, IMBE_FRAME_SIZE); - if (m_uiCodecOut2 == CODEC_CODEC2) - m_VocodecChannel->GetCodec2Data(Codec2); - if (m_uiCodecOut3 == CODEC_IMBE) - m_VocodecChannel->GetImbeData(Imbe); - - EncodeDvFramePacket(&Buffer, m_pStagedPacket->GetPid(), m_pStagedPacket->GetAmbe(), Codec2, Imbe); - m_Socket.SendVoice(Buffer, m_Ip, m_uiPort); + // software codecs caught up — sent delete m_pStagedPacket; m_pStagedPacket = NULL; } @@ -590,22 +683,9 @@ void CStream::Task(void) packet = (CAmbePacket *)queue->front(); queue->pop(); - // check if software codecs are ready (peek, don't consume) - bool codec2Ready = (m_uiCodecOut2 != CODEC_CODEC2) || m_VocodecChannel->HasCodec2Data(); - bool imbeReady = (m_uiCodecOut3 != CODEC_IMBE) || m_VocodecChannel->HasImbeData(); - - if ( codec2Ready && imbeReady ) + // all codecs ready — send immediately + if ( SendDvFramePacket(packet) ) { - // all codecs ready — get the data and send immediately - ::memset(Codec2, 0, CODEC2_FRAME_SIZE); - ::memset(Imbe, 0, IMBE_FRAME_SIZE); - if (m_uiCodecOut2 == CODEC_CODEC2) - m_VocodecChannel->GetCodec2Data(Codec2); - if (m_uiCodecOut3 == CODEC_IMBE) - m_VocodecChannel->GetImbeData(Imbe); - - EncodeDvFramePacket(&Buffer, packet->GetPid(), packet->GetAmbe(), Codec2, Imbe); - m_Socket.SendVoice(Buffer, m_Ip, m_uiPort); delete packet; } else @@ -944,18 +1024,146 @@ void CStream::TaskImbeInput(void) } while (ready); } +//////////////////////////////////////////////////////////////////////////////////////// +// extended Codec2, IMBE or PCM input handling + +void CStream::TaskSoftwareInput(void) +{ + CBuffer Buffer; + CIp Ip; + uint8 uiPid; + uint8 Frame[PCM_FRAME_SIZE]; + uint8 Ambe[AMBE_FRAME_SIZE]; + uint8 Codec2[CODEC2_FRAME_SIZE]; + uint8 Imbe[IMBE_FRAME_SIZE]; + uint8 Pcm[PCM_FRAME_SIZE]; + int16_t pcmNative[160]; + int16_t pcmAudio[160]; + int16_t pcmScaled[160]; + CVocodecChannel *Channels[] = { m_EncoderChannel1, m_EncoderChannel2 }; + CSignalProcessor *Processors[] = { m_pSignalProcessor1, m_pSignalProcessor2 }; + + // anything coming in from codec client ? + if ( (m_Socket.Receive(&Buffer, &Ip, 1) != -1) && IsValidDvFramePacket(Buffer, &uiPid, Frame) ) + { + m_LastActivity.Now(); + m_iTotalPackets++; + + // Software decode to native PCM (PCM input is S16LE) + if ( m_uiCodecIn == CODEC_CODEC2 ) + { + m_pCodec2Decoder->codec2_decode(pcmNative, Frame); + } + else if ( m_uiCodecIn == CODEC_IMBE ) + { + m_pImbeDecoder->decode_4400(pcmNative, Frame); + } + else + { + for (int i = 0; i < 160; i++) + pcmNative[i] = (int16_t)(Frame[i*2] | (Frame[i*2 + 1] << 8)); + } + + // PCM output is the native decoded speech, S16LE + for (int i = 0; i < 160; i++) + { + Pcm[i*2] = (uint8)((uint16_t)pcmNative[i] & 0xFF); + Pcm[i*2 + 1] = (uint8)(((uint16_t)pcmNative[i] >> 8) & 0xFF); + } + + // decode gain, then software encoders (m_fImbeGain is the IMBE decode gain for IMBE input) + ScalePcm(pcmNative, pcmAudio, (m_uiCodecIn == CODEC_IMBE) ? m_fImbeGain : m_fCodec2DecodeGain); + if ( m_pCodec2Encoder != NULL ) + { + ScalePcm(pcmAudio, pcmScaled, m_fCodec2Gain); + m_pCodec2Encoder->codec2_encode(Codec2, pcmScaled); + } + if ( m_pImbeEncoder != NULL ) + { + ScalePcm(pcmAudio, pcmScaled, m_fImbeGain); + m_pImbeEncoder->encode_4400(pcmScaled, Imbe); + } + + // reply in open order, AMBE slots are filled when the encoder channels return + ::memset(Ambe, 0, AMBE_FRAME_SIZE); + uint8 *Frames[] = { NULL, Ambe, Ambe, Codec2, Imbe, Pcm }; + EncodeDvFramePacket(&Buffer, uiPid, Frames); + if ( (m_EncoderChannel1 == NULL) && (m_EncoderChannel2 == NULL) ) + { + m_Socket.SendVoice(Buffer, m_Ip, m_uiPort); + } + else + { + // drop oldest if queue exceeds max depth (hardware stall protection) + if ( m_ReplyQueue.size() >= SOFTWARE_QUEUE_MAX_DEPTH ) + { + m_ReplyQueue.pop(); + } + m_ReplyQueue.push(Buffer); + + // push PCM to the encoder channels, DVSI hardware expects big-endian PCM + for ( int c = 0; c < 2; c++ ) + { + if ( Channels[c] != NULL ) + { + uint8 pcmBytes[PCM_FRAME_SIZE]; + for (int i = 0; i < 160; i++) + { + pcmBytes[i*2] = (uint8)(((uint16_t)pcmAudio[i] >> 8) & 0xFF); + pcmBytes[i*2 + 1] = (uint8)((uint16_t)pcmAudio[i] & 0xFF); + } + if ( Processors[c] != NULL ) + { + Processors[c]->Process(pcmBytes, PCM_FRAME_SIZE); + } + CVoicePacket *voicePacket = new CVoicePacket(pcmBytes, PCM_FRAME_SIZE); + voicePacket->SetChannel(Channels[c]->GetChannelOut()); + Channels[c]->PushPid(uiPid); + CPacketQueue *queue = Channels[c]->GetVoiceQueue(); + queue->push(voicePacket); + Channels[c]->ReleaseVoiceQueue(); + } + } + } + } + + // send queued replies once every open encoder channel has its frame back + // TOCTOU safety: only this thread pops from these queues; USB threads only push + for ( ;; ) + { + bool ready = !m_ReplyQueue.empty(); + for ( int c = 0; (c < 2) && ready; c++ ) + { + if ( Channels[c] != NULL ) + { + ready = !Channels[c]->GetPacketQueueOut()->empty(); + Channels[c]->ReleasePacketQueueOut(); + } + } + if ( !ready ) + { + break; + } + PatchDvFramePacket(&m_ReplyQueue.front(), m_EncoderChannel1, CODEC_AMBEPLUS); + PatchDvFramePacket(&m_ReplyQueue.front(), m_EncoderChannel2, CODEC_AMBE2PLUS); + m_Socket.SendVoice(m_ReplyQueue.front(), m_Ip, m_uiPort); + m_ReplyQueue.pop(); + } +} + //////////////////////////////////////////////////////////////////////////////////////// // packet decoding helpers -bool CStream::IsValidDvFramePacket(const CBuffer &Buffer, uint8 *pid, uint8 *ambe) +bool CStream::IsValidDvFramePacket(const CBuffer &Buffer, uint8 *pid, uint8 *frame) { bool valid = false; - if ( Buffer.size() == 11 ) + // codec(1) + pid(1) + frame of the input codec + if ( Buffer.size() == (size_t)(2 + FrameSize(GetCodecIn())) ) { uint8 codec = Buffer.data()[0]; *pid = Buffer.data()[1]; - ::memcpy(ambe, &(Buffer.data()[2]), 9); + ::memcpy(frame, &(Buffer.data()[2]), Buffer.size() - 2); valid = (codec == GetCodecIn()); } @@ -1017,3 +1225,95 @@ void CStream::EncodeDvFramePacket(CBuffer *Buffer, uint8 Pid, uint8 *Ambe, uint8 Buffer->Append(Imbe, IMBE_FRAME_SIZE); } +void CStream::EncodeDvFramePacket(CBuffer *Buffer, uint8 Pid, uint8 *Frames[]) +{ + uint8 Codecs[] = { m_uiCodecOut1, m_uiCodecOut2, m_uiCodecOut3, m_uiCodecOut4 }; + + // Extended response format, outputs in open order (Frames indexed by codec): + // codec1(1) + pid(1) + frame1 + { codecN(1) + frameN } for each further output + Buffer->clear(); + for ( int i = 0; (i < STREAM_MAX_OUTPUTS) && (Codecs[i] != CODEC_NONE); i++ ) + { + Buffer->Append((uint8)Codecs[i]); + if ( i == 0 ) + { + Buffer->Append((uint8)Pid); + } + Buffer->Append(Frames[Codecs[i]], FrameSize(Codecs[i])); + } +} + +bool CStream::SendDvFramePacket(CAmbePacket *packet) +{ + CBuffer Buffer; + uint8 Codec2[CODEC2_FRAME_SIZE]; + uint8 Imbe[IMBE_FRAME_SIZE]; + uint8 Pcm[PCM_FRAME_SIZE]; + + // check if software codecs are ready (peek, don't consume) + bool codec2Ready = !HasCodecOut(CODEC_CODEC2) || m_VocodecChannel->HasCodec2Data(); + bool imbeReady = !HasCodecOut(CODEC_IMBE) || m_VocodecChannel->HasImbeData(); + bool pcmReady = !HasCodecOut(CODEC_PCM_S16LE_8K) || m_VocodecChannel->HasPcmData(); + if ( !(codec2Ready && imbeReady && pcmReady) ) + { + return false; + } + + // get the data and send + ::memset(Codec2, 0, CODEC2_FRAME_SIZE); + ::memset(Imbe, 0, IMBE_FRAME_SIZE); + if ( HasCodecOut(CODEC_CODEC2) ) + m_VocodecChannel->GetCodec2Data(Codec2); + if ( HasCodecOut(CODEC_IMBE) ) + m_VocodecChannel->GetImbeData(Imbe); + if ( m_bExtended ) + { + // the hardware frame is the other AMBE codec, input codec is never an output + uint8 *Frames[] = { NULL, packet->GetAmbe(), packet->GetAmbe(), Codec2, Imbe, Pcm }; + if ( HasCodecOut(CODEC_PCM_S16LE_8K) ) + m_VocodecChannel->GetPcmData(Pcm); + EncodeDvFramePacket(&Buffer, packet->GetPid(), Frames); + } + else + { + EncodeDvFramePacket(&Buffer, packet->GetPid(), packet->GetAmbe(), Codec2, Imbe); + } + m_Socket.SendVoice(Buffer, m_Ip, m_uiPort); + return true; +} + +void CStream::PatchDvFramePacket(CBuffer *Buffer, CVocodecChannel *Channel, uint8 Codec) +{ + uint8 Codecs[] = { m_uiCodecOut1, m_uiCodecOut2, m_uiCodecOut3, m_uiCodecOut4 }; + + // pop the next hardware AMBE frame into its slot of a queued extended reply + if ( Channel != NULL ) + { + int offset = 2; + for ( int i = 0; (i < STREAM_MAX_OUTPUTS) && (Codecs[i] != Codec); i++ ) + { + offset += FrameSize(Codecs[i]) + 1; + } + CPacketQueue *queue = Channel->GetPacketQueueOut(); + CAmbePacket *packet = (CAmbePacket *)queue->front(); + queue->pop(); + Channel->ReleasePacketQueueOut(); + ::memcpy(Buffer->data() + offset, packet->GetAmbe(), AMBE_FRAME_SIZE); + delete packet; + } +} + +//////////////////////////////////////////////////////////////////////////////////////// +// codec helpers + +bool CStream::HasCodecOut(uint8 uiCodec) const +{ + // legacy streams only carry software Codec2 as 2nd and IMBE as 3rd output + if ( !m_bExtended ) + { + return ((uiCodec == CODEC_CODEC2) && (m_uiCodecOut2 == CODEC_CODEC2)) || + ((uiCodec == CODEC_IMBE) && (m_uiCodecOut3 == CODEC_IMBE)); + } + return (m_uiCodecOut1 == uiCodec) || (m_uiCodecOut2 == uiCodec) || + (m_uiCodecOut3 == uiCodec) || (m_uiCodecOut4 == uiCodec); +} diff --git a/ambed/cstream.h b/ambed/cstream.h index 4480bf8..094a89c 100644 --- a/ambed/cstream.h +++ b/ambed/cstream.h @@ -43,7 +43,7 @@ class CStream public: // constructors CStream(); - CStream(uint16, const CCallsign &, const CIp &, uint8, uint8, uint8, uint8); + CStream(uint16, const CCallsign &, const CIp &, uint8, uint8, uint8, uint8, uint8 = CODEC_NONE); // destructor virtual ~CStream(); @@ -60,6 +60,14 @@ public: uint8 GetCodecOut1(void) const { return m_uiCodecOut1; } uint8 GetCodecOut2(void) const { return m_uiCodecOut2; } uint8 GetCodecOut3(void) const { return m_uiCodecOut3; } + bool HasCodecOut(uint8) const; + + // extended streams (24 bytes open) + void SetTransaction(uint32 ui) { m_bExtended = true; m_uiTransaction = ui; } + bool IsExtended(void) const { return m_bExtended; } + bool IsTransaction(const CIp &Ip, uint32 ui, uint8 in, const uint8 *out) const + { return m_bExtended && (m_Ip == Ip) && (m_uiTransaction == ui) && (m_uiCodecIn == in) && + (m_uiCodecOut1 == out[0]) && (m_uiCodecOut2 == out[1]) && (m_uiCodecOut3 == out[2]) && (m_uiCodecOut4 == out[3]); } // activity timer bool IsActive(void) const { return m_LastActivity.DurationSinceNow() <= STREAM_ACTIVITY_TIMEOUT; } @@ -76,6 +84,9 @@ protected: // packet encoding helpers void EncodeDvFramePacket(CBuffer *, uint8, uint8 *, uint8 *, uint8 *); + void EncodeDvFramePacket(CBuffer *, uint8, uint8 *[]); + bool SendDvFramePacket(CAmbePacket *); + void PatchDvFramePacket(CBuffer *, CVocodecChannel *, uint8); // Codec2 input handling void TaskCodec2Input(void); @@ -83,6 +94,9 @@ protected: // IMBE input handling void TaskImbeInput(void); + // extended Codec2, IMBE or PCM input handling + void TaskSoftwareInput(void); + protected: // data uint16 m_uiId; @@ -92,6 +106,9 @@ protected: uint8 m_uiCodecOut1; uint8 m_uiCodecOut2; uint8 m_uiCodecOut3; + uint8 m_uiCodecOut4; + bool m_bExtended; + uint32 m_uiTransaction; CVocodecChannel *m_VocodecChannel; // Codec2 input mode: two encoder channels + decoder + signal processors + IMBE encoder @@ -108,6 +125,9 @@ protected: CCodec2 *m_pCodec2Encoder; // Software Codec2 encoder for IMBE input std::queue> m_Codec2Queue; // Queue for Codec2 data in IMBE input mode + // extended software input: replies waiting for hardware AMBE frames + std::queue m_ReplyQueue; + // Precomputed gain multipliers (avoid pow() per packet) float m_fCodec2DecodeGain; // Codec2 decode output gain (Codec2 input mode) float m_fImbeGain; // IMBE encode/decode gain diff --git a/ambed/cvocodecchannel.cpp b/ambed/cvocodecchannel.cpp index 313bf8f..db64629 100644 --- a/ambed/cvocodecchannel.cpp +++ b/ambed/cvocodecchannel.cpp @@ -53,6 +53,9 @@ CVocodecChannel::CVocodecChannel(CVocodecInterface *InterfaceIn, int iChIn, CVoc m_pImbeThread = NULL; m_bImbeStopThread = false; + // PCM tap starts disabled, enabled per-stream as needed + m_bPcmEnabled = false; + // PID FIFO desync drop counters — reset by PurgeAllQueues at // every Open/Close so each stream gets a fresh accounting epoch. m_PidFifoOrphanDrops = 0; @@ -146,6 +149,27 @@ uint8 CVocodecChannel::GetCodecOut(void) const void CVocodecChannel::ProcessSignal(CVoicePacket& voicePacket) { + // PCM tap takes the native decoded speech, before gain/AGC/filter + if (m_bPcmEnabled && (voicePacket.GetVoiceSize() >= PCM_FRAME_SIZE)) + { + // Convert big-endian PCM to little-endian + uint8 *rawVoice = voicePacket.GetVoice(); + std::vector pcm(PCM_FRAME_SIZE); + for (int i = 0; i < 160; i++) + { + pcm[i*2] = rawVoice[i*2 + 1]; + pcm[i*2 + 1] = rawVoice[i*2]; + } + + // Push to PCM queue (drop oldest if full) + std::lock_guard lock(m_PcmMutex); + if ( m_PcmQueue.size() >= SOFTWARE_QUEUE_MAX_DEPTH ) + { + m_PcmQueue.pop(); + } + m_PcmQueue.push(pcm); + } + m_signalProcessor->Process(voicePacket.GetVoice(), voicePacket.GetVoiceSize()); // Codec2 encoding runs in a separate thread to avoid blocking the hardware interface. @@ -421,6 +445,40 @@ bool CVocodecChannel::GetImbeData(uint8 *imbe) return true; } +//////////////////////////////////////////////////////////////////////////////////////// +// PCM tap control & queue access + +void CVocodecChannel::EnablePcm(bool bEnable) +{ + m_bPcmEnabled = bEnable; + if (!bEnable) + { + std::lock_guard lock(m_PcmMutex); + while (!m_PcmQueue.empty()) + { + m_PcmQueue.pop(); + } + } +} + +bool CVocodecChannel::HasPcmData(void) +{ + std::lock_guard lock(m_PcmMutex); + return !m_PcmQueue.empty(); +} + +bool CVocodecChannel::GetPcmData(uint8 *pcm) +{ + std::lock_guard lock(m_PcmMutex); + if (m_PcmQueue.empty()) + { + return false; + } + ::memcpy(pcm, m_PcmQueue.front().data(), PCM_FRAME_SIZE); + m_PcmQueue.pop(); + return true; +} + //////////////////////////////////////////////////////////////////////////////////////// // Async IMBE encoder thread @@ -538,6 +596,15 @@ void CVocodecChannel::PurgeAllQueues(void) } } + // Clear PCM tap queue + { + std::lock_guard lock(m_PcmMutex); + while (!m_PcmQueue.empty()) + { + m_PcmQueue.pop(); + } + } + // Clear PID-preservation FIFO and the desync drop counters. // Important: any pids left in the FIFO here would mis-pair with // future input/output once the channel re-opens, because the diff --git a/ambed/cvocodecchannel.h b/ambed/cvocodecchannel.h index 6cc4157..bed03e8 100644 --- a/ambed/cvocodecchannel.h +++ b/ambed/cvocodecchannel.h @@ -165,6 +165,11 @@ public: bool GetImbeData(uint8 *imbe); bool IsImbeEnabled(void) const { return m_bImbeEnabled; } + // PCM queue (native decoded speech, S16LE) + void EnablePcm(bool bEnable); + bool HasPcmData(void); + bool GetPcmData(uint8 *pcm); + // operators //virtual bool operator ==(const CVocodecChannel &) const { return false; } @@ -256,6 +261,11 @@ protected: static void ImbeEncoderThread(CVocodecChannel *pChannel); void ImbeEncoderTask(void); + // PCM tap (decoded speech before signal processing) + std::atomic m_bPcmEnabled; + std::mutex m_PcmMutex; + std::queue> m_PcmQueue; + private: CSignalProcessor* m_signalProcessor; }; diff --git a/ambed/main.h b/ambed/main.h index ef1fd3c..cfaf217 100644 --- a/ambed/main.h +++ b/ambed/main.h @@ -73,11 +73,18 @@ #define CODEC_AMBE2PLUS 2 #define CODEC_CODEC2 3 #define CODEC_IMBE 4 +#define CODEC_PCM_S16LE_8K 5 // extended streams only // Frame sizes -------------------------------------------------- #define AMBE_FRAME_SIZE 9 #define CODEC2_FRAME_SIZE 8 #define IMBE_FRAME_SIZE 11 +#define PCM_FRAME_SIZE 320 // 160 samples S16LE 8 kHz + +// Extended streams (24 bytes open) ----------------------------- +#define CAPS_VERSION 1 +#define STREAM_MAX_OUTPUTS 4 +#define SOFTWARE_MAX_STREAMS 4 // software only streams when no vocodec hardware // Transcoding speech gains (hardware vocodec channels: AMBE decode -> PCM -> AMBE encode) #define CODECGAIN_AMBEPLUS -10 // in dB (D-Star) diff --git a/src/ccodecstream.cpp b/src/ccodecstream.cpp index 3aa3c06..118e985 100644 --- a/src/ccodecstream.cpp +++ b/src/ccodecstream.cpp @@ -85,16 +85,17 @@ static bool BoundedJoin(std::thread *t, int timeoutMs) //////////////////////////////////////////////////////////////////////////////////////// // constructor -CCodecStream::CCodecStream(CPacketStream *PacketStream, uint16 uiId, uint8 uiCodecIn, uint8 uiCodecOut1, uint8 uiCodecOut2, uint8 uiCodecOut3) +CCodecStream::CCodecStream(CPacketStream *PacketStream, uint16 uiId, uint8 uiCodecIn, uint8 uiCodecOut1, uint8 uiCodecOut2, uint8 uiCodecOut3, uint8 uiCodecOut4) { m_bStopThread = false; m_pThread = NULL; m_uiStreamId = uiId; m_uiPid = 0; m_uiCodecIn = uiCodecIn; - m_uiCodecOut1 = uiCodecOut1; - m_uiCodecOut2 = uiCodecOut2; - m_uiCodecOut3 = uiCodecOut3; + m_uiCodecOut[0] = uiCodecOut1; + m_uiCodecOut[1] = uiCodecOut2; + m_uiCodecOut[2] = uiCodecOut3; + m_uiCodecOut[3] = uiCodecOut4; m_bConnected = false; m_fPingMin = -1; m_fPingMax = -1; @@ -590,9 +591,7 @@ void CCodecStream::Task(void) { CBuffer Buffer; CIp Ip; - uint8 Ambe1[AMBE_FRAME_SIZE]; - uint8 Ambe2[AMBE_FRAME_SIZE]; - uint8 Ambe3[IMBE_SIZE]; + const uint8 *Frames[TRANSCODER_MAX_OUTPUTS]; uint8 DStarSync[] = { 0x55,0x2D,0x16 }; // ---- Phase 1: handle ambed response (lookup + slot-fill in place) ---- @@ -610,7 +609,7 @@ void CCodecStream::Task(void) // regardless of arrival order or interleaved drops. if ( m_Socket.Receive(&Buffer, &Ip, 5) != -1 ) { - if ( IsValidAmbePacket(Buffer, Ambe1, Ambe2, Ambe3) ) + if ( IsValidAmbePacket(Buffer, Frames) ) { m_TimeoutTimer.Now(); @@ -676,7 +675,7 @@ void CCodecStream::Task(void) // HasAmbePlusData() ↔ m_bHasAmbePlus ↔ AMBE+2 (DMR family) // The "Plus" suffix in m_uiAmbePlus refers to "+2", not // "+". Easy to misread; don't swap them. - auto fillIfMissing = [&](uint8 codec, uint8 *data) { + auto fillIfMissing = [&](uint8 codec, const uint8 *data) { switch (codec) { case CODEC_AMBEPLUS: @@ -695,11 +694,16 @@ void CCodecStream::Task(void) if ( !Packet->HasImbeData() ) Packet->SetAmbe(codec, data); break; + case CODEC_PCM_S16LE_8K: + if ( !Packet->HasPcmData() ) + Packet->SetAmbe(codec, data); + break; } }; - fillIfMissing(m_uiCodecOut1, Ambe1); - fillIfMissing(m_uiCodecOut2, Ambe2); - fillIfMissing(m_uiCodecOut3, Ambe3); + for ( int i = 0; i < TRANSCODER_MAX_OUTPUTS; i++ ) + { + fillIfMissing(m_uiCodecOut[i], Frames[i]); + } m_PendingTranscode.erase(it); } else @@ -973,7 +977,7 @@ void CCodecStream::Task(void) // // m_SlowData and m_uiSlowDataCycle are touched only here in // Phase 3 of Task() — single-threaded, no lock needed. - if ( m_uiCodecOut1 == CODEC_AMBEPLUS || m_uiCodecOut2 == CODEC_AMBEPLUS ) + if ( HasCodecOut(CODEC_AMBEPLUS) ) { // Lazy-init on the first AMBE+ output frame of the stream — // the header is guaranteed to be cached in the packet @@ -1193,124 +1197,32 @@ void CCodecStream::Task(void) //////////////////////////////////////////////////////////////////////////////////////// /// packet decoding helpers -bool CCodecStream::IsValidAmbePacket(const CBuffer &Buffer, uint8 *Ambe1, uint8 *Ambe2, uint8 *Ambe3) +bool CCodecStream::IsValidAmbePacket(const CBuffer &Buffer, const uint8 **Frames) { - bool valid = false; - - // New 3-codec response format: - // codec1(1) + pid(1) + ambe1(9) + codec2(1) + codec2_data(8) + codec3(1) + imbe(11) = 32 bytes - if ( Buffer.size() == TRANSCODER_PACKET_SIZE_3CODEC ) - { - uint8 codec1 = Buffer.data()[TRANSCODER_PACKET_CODEC_OFFSET]; - uint8 codec2 = Buffer.data()[TRANSCODER_PACKET_CODEC2_OFFSET]; - uint8 codec3 = Buffer.data()[TRANSCODER_PACKET_CODEC3_OFFSET]; - - // verify codecs match what we requested - if ( codec1 == m_uiCodecOut1 && codec2 == m_uiCodecOut2 && codec3 == m_uiCodecOut3 ) - { - // first output is 9 bytes (AMBE+ or AMBE2+) - ::memcpy(Ambe1, &(Buffer.data()[TRANSCODER_PACKET_AMBE1_OFFSET]), AMBE_FRAME_SIZE); - // second output is 8 bytes (Codec2) - ::memcpy(Ambe2, &(Buffer.data()[TRANSCODER_PACKET_AMBE2_OFFSET]), CODEC2_FRAME_SIZE); - // third output is 11 bytes (IMBE) - ::memcpy(Ambe3, &(Buffer.data()[TRANSCODER_PACKET_AMBE3_OFFSET]), IMBE_SIZE); - valid = true; - } - } - // IMBE input response: codec1(1) + pid(1) + ambe1(9) + codec2(1) + ambe2(9) + codec3(1) + codec2_data(8) = 30 bytes - // Outputs are AMBE+, AMBE2+, and Codec2 for D-Star, DMR, M17 - // Note: IMBEIN has different offsets than 3CODEC because ambe2 is 9 bytes (vs 8 byte codec2_data in 3CODEC) - // IMBEIN: codec3 at offset 21, codec2_data at offset 22 - // 3CODEC: codec3 at offset 20, imbe at offset 21 - else if ( Buffer.size() == TRANSCODER_PACKET_SIZE_IMBEIN ) - { - uint8 codec1 = Buffer.data()[TRANSCODER_PACKET_CODEC_OFFSET]; - uint8 codec2 = Buffer.data()[TRANSCODER_PACKET_CODEC2_OFFSET]; - uint8 codec3 = Buffer.data()[21]; // IMBEIN: codec3 at offset 21 (after 9-byte ambe2) - - // verify codecs match what we requested - if ( codec1 == m_uiCodecOut1 && codec2 == m_uiCodecOut2 && codec3 == m_uiCodecOut3 ) - { - // first output is 9 bytes (AMBE+) - ::memcpy(Ambe1, &(Buffer.data()[TRANSCODER_PACKET_AMBE1_OFFSET]), AMBE_FRAME_SIZE); - // second output is 9 bytes (AMBE2+) - ::memcpy(Ambe2, &(Buffer.data()[TRANSCODER_PACKET_AMBE2_OFFSET]), AMBE_FRAME_SIZE); - // third output is 8 bytes (Codec2) at offset 22 - ::memcpy(Ambe3, &(Buffer.data()[22]), CODEC2_FRAME_SIZE); - valid = true; - } - } - // Codec2 input 3-codec response: codec1(1) + pid(1) + ambe1(9) + codec2(1) + ambe2(9) + codec3(1) + imbe(11) = 33 bytes - // Outputs are AMBE+, AMBE2+, and IMBE for D-Star, DMR, P25 - else if ( Buffer.size() == TRANSCODER_PACKET_SIZE_C2IN_3CODEC ) - { - uint8 codec1 = Buffer.data()[TRANSCODER_PACKET_CODEC_OFFSET]; - uint8 codec2 = Buffer.data()[TRANSCODER_PACKET_CODEC2_OFFSET]; - uint8 codec3 = Buffer.data()[21]; // C2IN_3CODEC: codec3 at offset 21 (after 9-byte ambe2) - - // verify codecs match what we requested - if ( codec1 == m_uiCodecOut1 && codec2 == m_uiCodecOut2 && codec3 == m_uiCodecOut3 ) - { - // first output is 9 bytes (AMBE+) - ::memcpy(Ambe1, &(Buffer.data()[TRANSCODER_PACKET_AMBE1_OFFSET]), AMBE_FRAME_SIZE); - // second output is 9 bytes (AMBE2+) - ::memcpy(Ambe2, &(Buffer.data()[TRANSCODER_PACKET_AMBE2_OFFSET]), AMBE_FRAME_SIZE); - // third output is 11 bytes (IMBE) at offset 22 - ::memcpy(Ambe3, &(Buffer.data()[22]), IMBE_SIZE); - valid = true; - } - } - // Codec2 input 2-codec response (legacy): codec1(1) + pid(1) + ambe1(9) + codec2(1) + ambe2(9) = 21 bytes - // Both outputs are AMBE codecs (AMBE+ and AMBE2+) - else if ( Buffer.size() == TRANSCODER_PACKET_SIZE_C2IN ) - { - uint8 codec1 = Buffer.data()[TRANSCODER_PACKET_CODEC_OFFSET]; - uint8 codec2 = Buffer.data()[TRANSCODER_PACKET_CODEC2_OFFSET]; - - // verify codecs match what we requested - if ( codec1 == m_uiCodecOut1 && codec2 == m_uiCodecOut2 ) - { - // first output is 9 bytes (AMBE+ or AMBE2+) - ::memcpy(Ambe1, &(Buffer.data()[TRANSCODER_PACKET_AMBE1_OFFSET]), AMBE_FRAME_SIZE); - // second output is also 9 bytes (AMBE+ or AMBE2+) - ::memcpy(Ambe2, &(Buffer.data()[TRANSCODER_PACKET_AMBE2_OFFSET]), AMBE_FRAME_SIZE); - // no third codec - ::memset(Ambe3, 0, IMBE_SIZE); - valid = true; - } - } - // AMBE input response: codec1(1) + pid(1) + ambe1(9) + codec2(1) + codec2(8) = 20 bytes - // First output is AMBE, second output is Codec2 - else if ( Buffer.size() == TRANSCODER_PACKET_SIZE_MULTI ) + // Response layout, outputs in open order: + // codec1(1) + pid(1) + frame1 [ + codec2(1) + frame2 [ + ... ] ] + // with each frame sized by its codec. ambed may return only the + // first n outputs (legacy 1/2/3-codec replies); the missing slots + // are zero filled, as the legacy fixed-size parsers did. + static const uint8 zero[PCM_FRAME_SIZE] = { 0 }; + size_t offset = 0; + int n = 0; + for ( ; (n < TRANSCODER_MAX_OUTPUTS) && (m_uiCodecOut[n] != CODEC_NONE) && (offset < Buffer.size()); n++ ) { - uint8 codec1 = Buffer.data()[TRANSCODER_PACKET_CODEC_OFFSET]; - uint8 codec2 = Buffer.data()[TRANSCODER_PACKET_CODEC2_OFFSET]; - - // verify codecs match what we requested - if ( codec1 == m_uiCodecOut1 && codec2 == m_uiCodecOut2 ) + size_t frame = offset + ((n == 0) ? 2 : 1); + size_t size = (size_t)GetCodecFrameSize(m_uiCodecOut[n]); + if ( (Buffer.data()[offset] != m_uiCodecOut[n]) || (frame + size > Buffer.size()) ) { - // first output is 9 bytes (AMBE+ or AMBE2+) - ::memcpy(Ambe1, &(Buffer.data()[TRANSCODER_PACKET_AMBE1_OFFSET]), AMBE_FRAME_SIZE); - // second output is 8 bytes (Codec2) - ::memcpy(Ambe2, &(Buffer.data()[TRANSCODER_PACKET_AMBE2_OFFSET]), CODEC2_FRAME_SIZE); - // no third codec - ::memset(Ambe3, 0, IMBE_SIZE); - valid = true; + return false; } + Frames[n] = &(Buffer.data()[frame]); + offset = frame + size; } - // Legacy single-codec response for backward compatibility - else if ( Buffer.size() == TRANSCODER_PACKET_SIZE_LEGACY ) + for ( int i = n; i < TRANSCODER_MAX_OUTPUTS; i++ ) { - uint8 codec = Buffer.data()[TRANSCODER_PACKET_CODEC_OFFSET]; - if ( codec == m_uiCodecOut1 ) - { - ::memcpy(Ambe1, &(Buffer.data()[TRANSCODER_PACKET_AMBE1_OFFSET]), AMBE_FRAME_SIZE); - ::memset(Ambe2, 0, CODEC2_FRAME_SIZE); - ::memset(Ambe3, 0, IMBE_SIZE); - valid = true; - } + Frames[i] = zero; } - return valid; + return (n > 0) && (offset == Buffer.size()); } //////////////////////////////////////////////////////////////////////////////////////// @@ -1321,21 +1233,36 @@ void CCodecStream::EncodeAmbePacket(CBuffer *Buffer, const uint8 *Ambe) Buffer->clear(); Buffer->Append(m_uiCodecIn); Buffer->Append(m_uiPid); + Buffer->Append((uint8 *)Ambe, GetCodecFrameSize(m_uiCodecIn)); - // Use correct size based on input codec - if ( m_uiCodecIn == CODEC_CODEC2 ) - { - Buffer->Append((uint8 *)Ambe, CODEC2_FRAME_SIZE); // 8 bytes for Codec2 - } - else if ( m_uiCodecIn == CODEC_IMBE ) + // increment PID for next packet (wraps at 256) + m_uiPid++; +} + +//////////////////////////////////////////////////////////////////////////////////////// +/// codec helpers + +int CCodecStream::GetCodecFrameSize(uint8 uiCodec) +{ + switch ( uiCodec ) { - Buffer->Append((uint8 *)Ambe, IMBE_SIZE); // 11 bytes for IMBE + case CODEC_AMBEPLUS: + case CODEC_AMBE2PLUS: return AMBE_FRAME_SIZE; + case CODEC_CODEC2: return CODEC2_FRAME_SIZE; + case CODEC_IMBE: return IMBE_FRAME_SIZE; + case CODEC_PCM_S16LE_8K: return PCM_FRAME_SIZE; + default: return 0; } - else +} + +bool CCodecStream::HasCodecOut(uint8 uiCodec) const +{ + for ( int i = 0; i < TRANSCODER_MAX_OUTPUTS; i++ ) { - Buffer->Append((uint8 *)Ambe, AMBE_FRAME_SIZE); // 9 bytes for AMBE + if ( m_uiCodecOut[i] == uiCodec ) + { + return true; + } } - - // increment PID for next packet (wraps at 256) - m_uiPid++; + return false; } diff --git a/src/ccodecstream.h b/src/ccodecstream.h index aa69d01..a5b2afa 100644 --- a/src/ccodecstream.h +++ b/src/ccodecstream.h @@ -200,21 +200,7 @@ class CDvFramePacket; // pointer-only use for m_pLastVoiceInJitter #define IMBE_SIZE 11 // transcoder packet offsets -#define TRANSCODER_PACKET_CODEC_OFFSET 0 #define TRANSCODER_PACKET_PID_OFFSET 1 -#define TRANSCODER_PACKET_AMBE1_OFFSET 2 -#define TRANSCODER_PACKET_CODEC2_OFFSET 11 -#define TRANSCODER_PACKET_AMBE2_OFFSET 12 -#define TRANSCODER_PACKET_CODEC3_OFFSET 20 -#define TRANSCODER_PACKET_AMBE3_OFFSET 21 - -// transcoder packet sizes -#define TRANSCODER_PACKET_SIZE_LEGACY 11 // codec(1) + pid(1) + ambe(9) -#define TRANSCODER_PACKET_SIZE_MULTI 20 // codec1(1) + pid(1) + ambe1(9) + codec2(1) + codec2_data(8) -#define TRANSCODER_PACKET_SIZE_C2IN 21 // codec1(1) + pid(1) + ambe1(9) + codec2(1) + ambe2(9) -#define TRANSCODER_PACKET_SIZE_IMBEIN 30 // codec1(1) + pid(1) + ambe1(9) + codec2(1) + ambe2(9) + codec3(1) + codec2_data(8) -#define TRANSCODER_PACKET_SIZE_3CODEC 32 // codec1(1) + pid(1) + ambe1(9) + codec2(1) + codec2_data(8) + codec3(1) + imbe(11) -#define TRANSCODER_PACKET_SIZE_C2IN_3CODEC 33 // codec1(1) + pid(1) + ambe1(9) + codec2(1) + ambe2(9) + codec3(1) + imbe(11) //////////////////////////////////////////////////////////////////////////////////////// @@ -226,7 +212,7 @@ class CCodecStream : public CPacketQueue { public: // constructor - CCodecStream(CPacketStream *, uint16, uint8, uint8, uint8, uint8); + CCodecStream(CPacketStream *, uint16, uint8, uint8, uint8, uint8, uint8 = CODEC_NONE); // destructor virtual ~CCodecStream(); @@ -277,11 +263,15 @@ public: protected: // packet decoding helpers - bool IsValidAmbePacket(const CBuffer &, uint8 *, uint8 *, uint8 *); + bool IsValidAmbePacket(const CBuffer &, const uint8 **); // packet encoding helpers void EncodeAmbePacket(CBuffer *, const uint8 *); + // codec helpers + static int GetCodecFrameSize(uint8); + bool HasCodecOut(uint8) const; + protected: // data @@ -289,9 +279,7 @@ protected: uint16 m_uiPort; uint8 m_uiPid; uint8 m_uiCodecIn; - uint8 m_uiCodecOut1; - uint8 m_uiCodecOut2; - uint8 m_uiCodecOut3; + uint8 m_uiCodecOut[TRANSCODER_MAX_OUTPUTS]; // in open order, CODEC_NONE = unused // socket CIp m_Ip; diff --git a/src/cdvframepacket.cpp b/src/cdvframepacket.cpp index 4faf497..fef00a9 100644 --- a/src/cdvframepacket.cpp +++ b/src/cdvframepacket.cpp @@ -181,6 +181,24 @@ CDvFramePacket::CDvFramePacket(uint16 sid, const uint8 *imbe) m_bHasCodec2 = false; } +// pcm constructor + +CDvFramePacket::CDvFramePacket(uint16 sid, uint8 pid, const uint8 *pcm) +: CPacket(sid, pid) +{ + ::memset(m_uiAmbe, 0, sizeof(m_uiAmbe)); + ::memset(m_uiDvData, 0, sizeof(m_uiDvData)); + m_bHasAmbe = false; + ::memset(m_uiAmbePlus, 0, sizeof(m_uiAmbePlus)); + ::memset(m_uiDvSync, 0, sizeof(m_uiDvSync)); + m_bHasAmbePlus = false; + ::memset(m_uiCodec2, 0, sizeof(m_uiCodec2)); + m_bHasCodec2 = false; + ::memset(m_uiImbe, 0, sizeof(m_uiImbe)); + m_bHasImbe = false; + SetAmbe(CODEC_PCM_S16LE_8K, pcm); +} + // copy constructor CDvFramePacket::CDvFramePacket(const CDvFramePacket &DvFrame) @@ -196,6 +214,7 @@ CDvFramePacket::CDvFramePacket(const CDvFramePacket &DvFrame) m_bHasCodec2 = DvFrame.m_bHasCodec2; ::memcpy(m_uiImbe, DvFrame.m_uiImbe, sizeof(m_uiImbe)); m_bHasImbe = DvFrame.m_bHasImbe; + m_Pcm = DvFrame.m_Pcm; } //////////////////////////////////////////////////////////////////////////////////////// @@ -217,6 +236,7 @@ const uint8 *CDvFramePacket::GetAmbe(uint8 uiCodec) const case CODEC_AMBE2PLUS: return m_uiAmbePlus; case CODEC_CODEC2: return m_uiCodec2; case CODEC_IMBE: return m_uiImbe; + case CODEC_PCM_S16LE_8K: return GetPcm(); default: return NULL; } } @@ -229,7 +249,7 @@ void CDvFramePacket::SetDvData(uint8 *DvData) ::memcpy(m_uiDvData, DvData, sizeof(m_uiDvData)); } -void CDvFramePacket::SetAmbe(uint8 uiCodec, uint8 *Ambe) +void CDvFramePacket::SetAmbe(uint8 uiCodec, const uint8 *Ambe) { // Each byte-write also sets the matching Has* flag. The flag // assignment is bound to the same case branch as the memcpy so @@ -254,6 +274,14 @@ void CDvFramePacket::SetAmbe(uint8 uiCodec, uint8 *Ambe) ::memcpy(m_uiImbe, Ambe, sizeof(m_uiImbe)); m_bHasImbe = true; break; + case CODEC_PCM_S16LE_8K: + { + // new buffer: duplicates already routed keep the old one + std::shared_ptr> pcm = std::make_shared>(); + ::memcpy(pcm->data(), Ambe, PCM_FRAME_SIZE); + m_Pcm = pcm; + break; + } } } @@ -269,6 +297,10 @@ void CDvFramePacket::ConformAvailableCodecs(uint32 mask) m_bHasAmbePlus = (mask & (1U << CODEC_AMBE2PLUS)) != 0U; m_bHasCodec2 = (mask & (1U << CODEC_CODEC2)) != 0U; m_bHasImbe = (mask & (1U << CODEC_IMBE)) != 0U; + if ( (mask & (1U << CODEC_PCM_S16LE_8K)) == 0U ) + { + m_Pcm.reset(); + } } diff --git a/src/cdvframepacket.h b/src/cdvframepacket.h index 3a2f40d..5cdca33 100644 --- a/src/cdvframepacket.h +++ b/src/cdvframepacket.h @@ -25,6 +25,8 @@ #ifndef cdvframepacket_h #define cdvframepacket_h +#include +#include #include "cpacket.h" //////////////////////////////////////////////////////////////////////////////////////// @@ -66,6 +68,8 @@ public: CDvFramePacket(const uint8 *codec2, uint16 sid, uint8 pid); // P25 constructor CDvFramePacket(uint16 sid, const uint8 *imbe); + // PCM constructor + CDvFramePacket(uint16 sid, uint8 pid, const uint8 *pcm); CDvFramePacket(const CDvFramePacket &); // destructor @@ -86,6 +90,7 @@ public: const uint8 *GetImbe(void) const { return m_uiImbe; } const uint8 *GetDvData(void) const { return m_uiDvData; } const uint8 *GetDvSync(void) const { return m_uiDvSync; } + const uint8 *GetPcm(void) const { return m_Pcm ? m_Pcm->data() : NULL; } // Per-codec presence flags. Each Has* returns true iff the // corresponding byte slot has been populated with real codec // data (via SetAmbe or a real-data constructor). The default @@ -105,10 +110,11 @@ public: bool HasAmbePlusData(void) const { return m_bHasAmbePlus; } bool HasCodec2Data(void) const { return m_bHasCodec2; } bool HasImbeData(void) const { return m_bHasImbe; } + bool HasPcmData(void) const { return m_Pcm != nullptr; } // set void SetDvData(uint8 *); - void SetAmbe(uint8, uint8 *); + void SetAmbe(uint8, const uint8 *); void SetCodec2(const uint8 *); // Reconcile the four Has-flags to reflect what the source actually @@ -150,6 +156,9 @@ protected: // data (p25) uint8 m_uiImbe[IMBE_SIZE]; bool m_bHasImbe; + // data (pcm) - allocated only when present and shared read-only + // by the per-protocol duplicates, so non-PCM traffic pays nothing + std::shared_ptr> m_Pcm; }; diff --git a/src/creflector.cpp b/src/creflector.cpp index 94eca6b..e791081 100644 --- a/src/creflector.cpp +++ b/src/creflector.cpp @@ -324,10 +324,10 @@ CPacketStream *CReflector::OpenStream(CDvHeaderPacket *DvHeader, CClient *client // codec is missing) // // Source-codec preference when multiple are available: - // AMBE+2 > AMBE+ > Codec2 > IMBE + // AMBE+2 > AMBE+ > Codec2 > IMBE > PCM // (most-common cross-mode pairing first; IMBE only as input if // it's literally the only thing available, which would only - // happen for native P25 sources). + // happen for native P25 sources; PCM only for USRP sources). uint8 transcodeInputCodec = CODEC_NONE; if ( transcodeSourceCodecs != 0U ) { @@ -339,6 +339,7 @@ CPacketStream *CReflector::OpenStream(CDvHeaderPacket *DvHeader, CClient *client else if ( transcodeSourceCodecs & (1U << CODEC_AMBEPLUS) ) transcodeInputCodec = CODEC_AMBEPLUS; else if ( transcodeSourceCodecs & (1U << CODEC_CODEC2) ) transcodeInputCodec = CODEC_CODEC2; else if ( transcodeSourceCodecs & (1U << CODEC_IMBE) ) transcodeInputCodec = CODEC_IMBE; + else if ( transcodeSourceCodecs & (1U << CODEC_PCM_S16LE_8K) ) transcodeInputCodec = CODEC_PCM_S16LE_8K; } } // Backward-compat fallback: if the inputCodec from the stream is @@ -352,10 +353,19 @@ CPacketStream *CReflector::OpenStream(CDvHeaderPacket *DvHeader, CClient *client transcodeInputCodec = inputCodec; } + // PCM in or out needs the extended open, which carries exactly + // the missing codecs; everything else keeps the legacy open. CCodecStream *codecStream = NULL; if ( transcodeInputCodec != CODEC_NONE ) { - codecStream = g_Transcoder.GetStream(stream, transcodeInputCodec); + if ( (transcodeInputCodec == CODEC_PCM_S16LE_8K) || (neededCodecs & (1U << CODEC_PCM_S16LE_8K)) ) + { + codecStream = g_Transcoder.GetStream(stream, transcodeInputCodec, neededCodecs & ~transcodeSourceCodecs); + } + else + { + codecStream = g_Transcoder.GetStream(stream, transcodeInputCodec); + } } // ----- Phase 3: attach the codec stream (stream lock only) ----- diff --git a/src/ctranscoder.cpp b/src/ctranscoder.cpp index 4cf2c4c..fd89301 100644 --- a/src/ctranscoder.cpp +++ b/src/ctranscoder.cpp @@ -24,6 +24,7 @@ #include "main.h" #include +#include #include "creflector.h" #include "ctranscoder.h" @@ -36,6 +37,15 @@ // timeout — must accommodate remote AMBEd with 50-200ms network RTT #define AMBED_OPENSTREAM_TIMEOUT 1000 // in ms +#define AMBED_CAPS_TIMEOUT 1000 // in ms + +//////////////////////////////////////////////////////////////////////////////////////// +// helper + +static uint32 GetBE32(const uint8 *p) +{ + return ((uint32)p[0] << 24) | ((uint32)p[1] << 16) | ((uint32)p[2] << 8) | (uint32)p[3]; +} //////////////////////////////////////////////////////////////////////////////////////// @@ -57,6 +67,15 @@ CTranscoder::CTranscoder() m_bWaitingForStream = false; m_StreamidOpenStream = 0; m_PortOpenStream = 0; + m_uiOpenTransaction = 0; + // random start: a restarted xlxd does not reuse the previous run's ids + m_uiLastTransaction = (uint32)std::random_device()(); + m_bPcmWanted = false; + m_iPcmCaps = PCMCAPS_IDLE; + m_uiPcmMaxOutputs = 0; + m_uiPcmCodecs = 0; + m_bPcmUnavailableLogged = false; + m_bPcmPartialLogged = false; } //////////////////////////////////////////////////////////////////////////////////////// @@ -140,6 +159,9 @@ void CTranscoder::Task(void) CIp Ip; uint16 StreamId; uint16 Port; + uint32 Transaction; + uint8 MaxOutputs; + uint32 Codecs; // anything coming in from codec server ? //if ( (m_Socket.Receive(&Buffer, &Ip, 20) != -1) && (Ip == m_Ip) ) @@ -148,9 +170,9 @@ void CTranscoder::Task(void) m_LastActivityTime.Now(); // crack packet - if ( IsValidStreamDescrPacket(Buffer, &StreamId, &Port) ) + if ( IsValidStreamDescrPacket(Buffer, &StreamId, &Port, &Transaction) ) { - if ( m_bWaitingForStream ) + if ( m_bWaitingForStream && (Transaction == m_uiOpenTransaction) ) { // Normal path: GetStream is waiting for this response m_bStreamOpened = true; @@ -170,9 +192,9 @@ void CTranscoder::Task(void) << " — sent close to free hardware channel" << std::endl; } } - else if ( IsValidNoStreamAvailablePacket(Buffer) ) + else if ( IsValidNoStreamAvailablePacket(Buffer, &Transaction) ) { - if ( m_bWaitingForStream ) + if ( m_bWaitingForStream && (Transaction == m_uiOpenTransaction) ) { m_bStreamOpened = false; m_SemaphoreOpenStream.Notify(); @@ -184,10 +206,38 @@ void CTranscoder::Task(void) if ( !m_bConnected ) { std::cout << "Transcoder connected at " << Ip << std::endl; + // new connection, probe the extended open again + m_iPcmCaps = PCMCAPS_IDLE; } m_bConnected = true; } - + else if ( IsValidCapsPacket(Buffer, &MaxOutputs, &Codecs) ) + { + if ( m_iPcmCaps == PCMCAPS_PROBING ) + { + m_uiPcmMaxOutputs = MaxOutputs; + m_uiPcmCodecs = Codecs; + m_iPcmCaps = PCMCAPS_OK; + std::cout << "Transcoder extended open: " << (int)MaxOutputs << " outputs, codec mask 0x" + << std::hex << Codecs << std::dec << std::endl; + } + } + } + + // probe the extended open once per connection, and only when a PCM + // endpoint is configured: legacy setups never see the request + if ( m_bPcmWanted && m_bConnected && (m_iPcmCaps == PCMCAPS_IDLE) ) + { + EncodeCapsPacket(&Buffer); + m_Socket.Send(Buffer, m_Ip, TRANSCODER_PORT); + m_PcmCapsTime.Now(); + m_iPcmCaps = PCMCAPS_PROBING; + } + else if ( (m_iPcmCaps == PCMCAPS_PROBING) && (m_PcmCapsTime.DurationSinceNow() >= (AMBED_CAPS_TIMEOUT/1000.0)) ) + { + // no answer: legacy transcoder, keep that for this connection + m_iPcmCaps = PCMCAPS_NONE; + std::cout << "Transcoder does not answer CAPS, extended open disabled" << std::endl; } // keep client alive @@ -244,67 +294,143 @@ CCodecStream *CTranscoder::GetStream(CPacketStream *PacketStream, uint8 uiCodecI return NULL; } - // Serialize stream open requests — the shared m_bStreamOpened / - // m_StreamidOpenStream / m_PortOpenStream fields are single-use, - // so concurrent callers must wait their turn - Lock(); - - // send openstream request - m_bStreamOpened = false; - m_bWaitingForStream = true; + uint8 uiCodecOut[TRANSCODER_MAX_OUTPUTS] = { uiCodecOut1, uiCodecOut2, uiCodecOut3, CODEC_NONE }; EncodeOpenstreamPacket(&Buffer, uiCodecIn, uiCodecOut1, uiCodecOut2, uiCodecOut3); - m_SemaphoreOpenStream.PreWaitFor(); - m_Socket.Send(Buffer, m_Ip, TRANSCODER_PORT); + stream = OpenStream(PacketStream, Buffer, 0, uiCodecIn, uiCodecOut); + } + } + return stream; +} - // wait for AMBEd response (holding mutex blocks other GetStream callers) - bool gotResponse = m_SemaphoreOpenStream.WaitFor(AMBED_OPENSTREAM_TIMEOUT); +CCodecStream *CTranscoder::GetStream(CPacketStream *PacketStream, uint8 uiCodecIn, uint32 uiCodecsOut) +{ + CBuffer Buffer; - // If primary wait timed out, give a brief second chance to catch - // responses that were in-flight. This preserves the semaphore's - // happens-before ordering for m_StreamidOpenStream/m_PortOpenStream - // (reading those fields without the semaphore signal is a data race). - if ( !gotResponse ) - { - gotResponse = m_SemaphoreOpenStream.WaitFor(50); - } + // without extended open, fall back to the legacy open unless the + // input is PCM, which only the extended open can carry + if ( !IsPcmCapable(uiCodecIn) ) + { + if ( uiCodecIn != CODEC_PCM_S16LE_8K ) + { + return GetStream(PacketStream, uiCodecIn); + } + if ( !m_bPcmUnavailableLogged.exchange(true) ) + { + std::cout << "ambed has no extended open, PCM streams are not transcoded" << std::endl; + } + return NULL; + } - // Clear waiting flag — any later response from Task() will be - // detected as orphaned and closed via AMBEDCS - m_bWaitingForStream = false; + // ordered output list: ascending codec, input excluded, limited to + // what the transcoder announced + uint8 uiCodecOut[TRANSCODER_MAX_OUTPUTS] = { CODEC_NONE, CODEC_NONE, CODEC_NONE, CODEC_NONE }; + uint32 wanted = uiCodecsOut & ~(1U << uiCodecIn); + uint32 served = 0; + int n = 0; + for ( uint8 c = CODEC_AMBEPLUS; (c <= CODEC_PCM_S16LE_8K) && (n < (int)m_uiPcmMaxOutputs); c++ ) + { + if ( (wanted & m_uiPcmCodecs) & (1U << c) ) + { + uiCodecOut[n++] = c; + served |= (1U << c); + } + } + if ( (wanted & ~served) && !m_bPcmPartialLogged.exchange(true) ) + { + std::cout << "ambed extended open: codec mask 0x" << std::hex << (wanted & ~served) << std::dec << " not served" << std::endl; + } + if ( n == 0 ) + { + return NULL; + } - if ( gotResponse && m_bStreamOpened ) - { - std::cout << "ambed stream open on port " << m_PortOpenStream << std::endl; + // transaction ties the descriptor reply to this request, never 0 + uint32 uiTransaction = ++m_uiLastTransaction; + if ( uiTransaction == 0 ) + { + uiTransaction = ++m_uiLastTransaction; + } + EncodeOpenstreamPacket(&Buffer, uiCodecIn, uiCodecOut, uiTransaction); + return OpenStream(PacketStream, Buffer, uiTransaction, uiCodecIn, uiCodecOut); +} - stream = new CCodecStream(PacketStream, m_StreamidOpenStream, uiCodecIn, uiCodecOut1, uiCodecOut2, uiCodecOut3); +CCodecStream *CTranscoder::OpenStream(CPacketStream *PacketStream, const CBuffer &Request, uint32 uiTransaction, uint8 uiCodecIn, const uint8 *uiCodecOut) +{ + CBuffer Buffer; - if ( stream->Init(m_PortOpenStream) ) - { - m_Streams.push_back(stream); - } - else - { - EncodeClosestreamPacket(&Buffer, stream->GetStreamId()); - m_Socket.Send(Buffer, m_Ip, TRANSCODER_PORT); - delete stream; - stream = NULL; - } - } - else if ( gotResponse && !m_bStreamOpened ) - { - std::cout << "ambed openstream failed (no suitable channel available)" << std::endl; - } - else - { - std::cout << "ambed openstream timeout" << std::endl; - } + CCodecStream *stream = NULL; + + // Serialize stream open requests — the shared m_bStreamOpened / + // m_StreamidOpenStream / m_PortOpenStream fields are single-use, + // so concurrent callers must wait their turn + Lock(); + + // send openstream request + m_bStreamOpened = false; + m_uiOpenTransaction = uiTransaction; + m_bWaitingForStream = true; + m_SemaphoreOpenStream.PreWaitFor(); + m_Socket.Send(Request, m_Ip, TRANSCODER_PORT); + + // wait for AMBEd response (holding mutex blocks other GetStream callers) + bool gotResponse = m_SemaphoreOpenStream.WaitFor(AMBED_OPENSTREAM_TIMEOUT); + + // If primary wait timed out, give a brief second chance to catch + // responses that were in-flight. This preserves the semaphore's + // happens-before ordering for m_StreamidOpenStream/m_PortOpenStream + // (reading those fields without the semaphore signal is a data race). + if ( !gotResponse ) + { + gotResponse = m_SemaphoreOpenStream.WaitFor(50); + } + + // Clear waiting flag — any later response from Task() will be + // detected as orphaned and closed via AMBEDCS + m_bWaitingForStream = false; + + if ( gotResponse && m_bStreamOpened ) + { + std::cout << "ambed stream open on port " << m_PortOpenStream << std::endl; + + stream = new CCodecStream(PacketStream, m_StreamidOpenStream, uiCodecIn, + uiCodecOut[0], uiCodecOut[1], uiCodecOut[2], uiCodecOut[3]); - Unlock(); + if ( stream->Init(m_PortOpenStream) ) + { + m_Streams.push_back(stream); + } + else + { + EncodeClosestreamPacket(&Buffer, stream->GetStreamId()); + m_Socket.Send(Buffer, m_Ip, TRANSCODER_PORT); + delete stream; + stream = NULL; } } + else if ( gotResponse && !m_bStreamOpened ) + { + std::cout << "ambed openstream failed (no suitable channel available)" << std::endl; + } + else + { + std::cout << "ambed openstream timeout" << std::endl; + } + + Unlock(); return stream; } +bool CTranscoder::IsPcmCapable(uint8 uiCodecIn) +{ + // a stream opened right after connect waits for the pending probe + for ( int i = 0; (i <= AMBED_CAPS_TIMEOUT/10) && (m_iPcmCaps == PCMCAPS_PROBING); i++ ) + { + CTimePoint::TaskSleepFor(10); + } + uint32 needed = (1U << CODEC_PCM_S16LE_8K) | (1U << uiCodecIn); + return m_bConnected && (m_iPcmCaps == PCMCAPS_OK) && ((m_uiPcmCodecs & needed) == needed); +} + void CTranscoder::ReleaseStream(CCodecStream *stream) { CBuffer Buffer; @@ -440,27 +566,52 @@ bool CTranscoder::IsValidKeepAlivePacket(const CBuffer &Buffer) return valid; } -bool CTranscoder::IsValidStreamDescrPacket(const CBuffer &Buffer, uint16 *Id, uint16 *Port) +bool CTranscoder::IsValidStreamDescrPacket(const CBuffer &Buffer, uint16 *Id, uint16 *Port, uint32 *Transaction) { uint8 tag[] = { 'A','M','B','E','D','S','T','D' }; bool valid = false; - if ( (Buffer.size() == 14) && (Buffer.Compare(tag, sizeof(tag)) == 0) ) + // legacy 14 bytes, extended open reply adds a BE32 transaction + if ( ((Buffer.size() == 14) || (Buffer.size() == 18)) && (Buffer.Compare(tag, sizeof(tag)) == 0) ) { ::memcpy(Id, &Buffer.data()[8], sizeof(uint16)); ::memcpy(Port, &Buffer.data()[10], sizeof(uint16)); // uint8 CodecIn = Buffer.data()[12]; // uint8 CodecOut = Buffer.data()[13]; + *Transaction = (Buffer.size() == 18) ? GetBE32(&Buffer.data()[14]) : 0; valid = true; } return valid; } -bool CTranscoder::IsValidNoStreamAvailablePacket(const CBuffer&Buffer) +bool CTranscoder::IsValidNoStreamAvailablePacket(const CBuffer&Buffer, uint32 *Transaction) { uint8 tag[] = { 'A','M','B','E','D','B','U','S','Y' }; - return ( (Buffer.size() == 9) && (Buffer.Compare(tag, sizeof(tag)) == 0) ); + bool valid = false; + // legacy 9 bytes, extended open reply adds a BE32 transaction + if ( ((Buffer.size() == 9) || (Buffer.size() == 13)) && (Buffer.Compare(tag, sizeof(tag)) == 0) ) + { + *Transaction = (Buffer.size() == 13) ? GetBE32(&Buffer.data()[9]) : 0; + valid = true; + } + return valid; +} + +bool CTranscoder::IsValidCapsPacket(const CBuffer &Buffer, uint8 *MaxOutputs, uint32 *Codecs) +{ + uint8 tag[] = { 'A','M','B','E','D','C','A','P','S' }; + + // tag(9) + version 1 + max outputs (1..4) + BE32 mask of codecs 1..5 + bool valid = false; + if ( (Buffer.size() == 15) && (Buffer.Compare(tag, sizeof(tag)) == 0) && (Buffer.data()[9] == 1) && + (Buffer.data()[10] >= 1) && (Buffer.data()[10] <= TRANSCODER_MAX_OUTPUTS) ) + { + *MaxOutputs = Buffer.data()[10]; + *Codecs = GetBE32(&Buffer.data()[11]) & 0x3E; + valid = (*Codecs != 0); + } + return valid; } @@ -475,6 +626,13 @@ void CTranscoder::EncodeKeepAlivePacket(CBuffer *Buffer) Buffer->Append((uint8 *)(const char *)g_Reflector.GetCallsign(), CALLSIGN_LEN); } +void CTranscoder::EncodeCapsPacket(CBuffer *Buffer) +{ + uint8 tag[] = { 'A','M','B','E','D','C','A','P','S' }; + + Buffer->Set(tag, sizeof(tag)); +} + void CTranscoder::EncodeOpenstreamPacket(CBuffer *Buffer, uint8 uiCodecIn, uint8 uiCodecOut1, uint8 uiCodecOut2, uint8 uiCodecOut3) { uint8 tag[] = { 'A','M','B','E','D','O','S' }; @@ -487,6 +645,14 @@ void CTranscoder::EncodeOpenstreamPacket(CBuffer *Buffer, uint8 uiCodecIn, uint8 Buffer->Append((uint8)uiCodecOut3); } +void CTranscoder::EncodeOpenstreamPacket(CBuffer *Buffer, uint8 uiCodecIn, const uint8 *uiCodecOut, uint32 uiTransaction) +{ + // legacy 19-byte open + 4th output + BE32 transaction = 24 bytes + EncodeOpenstreamPacket(Buffer, uiCodecIn, uiCodecOut[0], uiCodecOut[1], uiCodecOut[2]); + Buffer->Append((uint8)uiCodecOut[3]); + Buffer->Append((uint32)htonl(uiTransaction)); +} + void CTranscoder::EncodeClosestreamPacket(CBuffer *Buffer, uint16 uiStreamId) { uint8 tag[] = { 'A','M','B','E','D','C','S' }; diff --git a/src/ctranscoder.h b/src/ctranscoder.h index dec422f..b3769f0 100644 --- a/src/ctranscoder.h +++ b/src/ctranscoder.h @@ -33,6 +33,11 @@ //////////////////////////////////////////////////////////////////////////////////////// // define +// extended open (PCM) capability of the connected transcoder +#define PCMCAPS_IDLE 0 // not probed on this connection +#define PCMCAPS_PROBING 1 // CAPS sent, waiting for reply +#define PCMCAPS_NONE 2 // no or invalid reply: legacy only +#define PCMCAPS_OK 3 // extended open available //////////////////////////////////////////////////////////////////////////////////////// // class @@ -58,9 +63,11 @@ public: // status bool IsConnected(void) const { return m_bConnected.load(); } + void SetPcmWanted(void) { m_bPcmWanted = true; } // manage streams CCodecStream *GetStream(CPacketStream *, uint8); + CCodecStream *GetStream(CPacketStream *, uint8, uint32); void ReleaseStream(CCodecStream *); // task @@ -68,17 +75,24 @@ public: void Task(void); protected: + // manage streams + CCodecStream *OpenStream(CPacketStream *, const CBuffer &, uint32, uint8, const uint8 *); + bool IsPcmCapable(uint8); + // keepalive helpers void HandleKeepalives(void); // packet decoding helpers bool IsValidKeepAlivePacket(const CBuffer &); - bool IsValidStreamDescrPacket(const CBuffer &, uint16 *, uint16 *); - bool IsValidNoStreamAvailablePacket(const CBuffer&); + bool IsValidStreamDescrPacket(const CBuffer &, uint16 *, uint16 *, uint32 *); + bool IsValidNoStreamAvailablePacket(const CBuffer&, uint32 *); + bool IsValidCapsPacket(const CBuffer &, uint8 *, uint32 *); // packet encoding helpers void EncodeKeepAlivePacket(CBuffer *); + void EncodeCapsPacket(CBuffer *); void EncodeOpenstreamPacket(CBuffer *, uint8, uint8, uint8, uint8); + void EncodeOpenstreamPacket(CBuffer *, uint8, const uint8 *, uint32); void EncodeClosestreamPacket(CBuffer *, uint16); protected: @@ -92,6 +106,17 @@ protected: std::atomic m_bWaitingForStream; // true while GetStream is waiting for a response std::atomic m_StreamidOpenStream; std::atomic m_PortOpenStream; + std::atomic m_uiOpenTransaction; // 0 = legacy open + std::atomic m_uiLastTransaction; + + // extended open capability, probed once per connection + std::atomic m_bPcmWanted; + std::atomic m_iPcmCaps; + std::atomic m_uiPcmMaxOutputs; + std::atomic m_uiPcmCodecs; + std::atomic m_bPcmUnavailableLogged; + std::atomic m_bPcmPartialLogged; + CTimePoint m_PcmCapsTime; // thread std::atomic m_bStopThread; diff --git a/src/main.h b/src/main.h index 429e199..717e2d8 100644 --- a/src/main.h +++ b/src/main.h @@ -207,11 +207,14 @@ static_assert(DSCP_VALUE >= 0 && DSCP_VALUE <= 63, #define CODEC_AMBE2PLUS 2 // DMR #define CODEC_CODEC2 3 // M17 #define CODEC_IMBE 4 // P25 +#define CODEC_PCM_S16LE_8K 5 // raw PCM (USRP) // transcoder packet sizes -------------------------------------- #define AMBE_FRAME_SIZE 9 // AMBE+/AMBE2+ frame #define CODEC2_FRAME_SIZE 8 // Codec2 3200bps frame #define IMBE_FRAME_SIZE 11 // IMBE 88-bit frame +#define PCM_FRAME_SIZE 320 // 160 samples S16LE at 8 kHz +#define TRANSCODER_MAX_OUTPUTS 4 // outputs per extended open #define TRANSCODER_PACKET_SIZE_LEGACY 11 // codec(1)+pid(1)+ambe(9) #define TRANSCODER_PACKET_SIZE_MULTI 20 // codec1(1)+pid(1)+ambe(9)+codec2(1)+codec2(8) #define TRANSCODER_PACKET_SIZE_C2IN 21 // codec1(1)+pid(1)+ambe(9)+codec2(1)+ambe(9) - Codec2 input